#include <cmath>
#include <float.h>   // DBL_MAX
#include <string.h>  // memcpy() strncpy()
#include <unistd.h>  // unlink()
#include <stdio.h>
#include <string>
#include <vector>
#include <fstream>
#include "rtapi/rtapi.h"
#include "config.h"
#include "motion/usrmotintf.h"  // usrmotInit(), usrmotReadZucmotStatus(), etc.
#include "motion/motion.h"      // zucmot_command_t,STATUS, etc.
#include "motion/motion_debug.h"
#include "nml_intf/zuc.hh"
#include "zuc/nml_intf/zucpos.h"
#include "nml_intf/zuccfg.h"  // ZUC_INIFILE
#include "nml_intf/zucglb.h"  // ZUC_INIFILE
#include "nml_intf/zuc_nml.hh"
#include "rcs_print.hh"
#include "timer.hh"
// #include "zucIniFile.hh"
#include "ini/iniaxis.hh"
#include "ini/inijoint.hh"
#include "ini/initraj.hh"
#include "jk_bl.h"
// #include "minIni.h"
#include <Python.h>
#include "taskjoint.hh"
#include <sys/ipc.h>
#include <sys/shm.h>
#include <sys/types.h>
#include <sys/wait.h>
#include <errno.h>
#include "errcode.h"
#include "loadidentify.hh"
#include <eigen3/Eigen/Dense>
#include "motion/safezone.h"
// #include <boost/property_tree/ini_parser.hpp>
#include <sstream>
#include "DYN_toppra.hh"
#include "time.h"
#include "pyCubic.h"
#include "kinematics/kinematics.h"
#include "rtdev/tio_gripper.h"
#include "rtdev/rs485msgque.h"
#include "rtdev/safetyboard/scbitf.h"
#include "zuc/ini/iniextio.hh"
#include "zuc/ini/initio.hh"
#include "zuc/ini/inirobmodule.hh"
#include <regex>
#include <boost/algorithm/string.hpp>
#include "log/zlog.hh"
#include "JakaDynIdentify.hh"
#include "motion/rtcmds.h"
#include "axisgroup/axisgroup.h"
#include "axisgroup/axisgroup_robot.h"
#include "kine/kine.h"
#include "rtbus/rtbus.h"
#include "motion_al.h"  //motion应用层接口
#include "safety/safety_cmd.h"
#include "dynamic/dynamic_cmd.h"
#include "motion/motion_cmd/motion_cmd.h"
#include "motion/axis_setting_cmd/axis_settings_cmd.h"
#include "motion/motion_cmd/rtdev_setting_cmd.h"
#include "motion/motion_cmd/moving_cmd.h"
#include "motion/motion_cmd/useless_cmd.h"
#include "motion/motion_cmd/force_control_cmd.h"
#include "jkutil/threadpool.h"
#include "rob_common/robot_payload.h"
#include "taskintf_config.h"
#include "taskintf.h"
#include "axisgroup/robot_mgr.h"
#include "rob_common/usrframe.h"
#include "rob_common/robtool.h"

bool has_special_chars(std::string text)
{
    std::regex pattern("[!@#$%^&*(),.?\":{}|<>]");
    return std::regex_search(text, pattern);
}

using namespace Eigen;

#define CRC_FILE (ZUC_USER_CONFIG_DIR "/checksum.ini")
// #define EN_TOPPRA 1
#define RPY_FUSS 0.0001
#define TCP_ERROR_TYPE 1
/*functions for kinematics in interp module*/
extern int getInvKineFlag(double* joint, unsigned long* curr_iflags);
extern int setInterpDHParam(DHParam dhparam);
static int usrOpeErrCode = -1;
static struct ZucPose utilCartPos;
static double utilJointPos[ZUCMOT_MAX_JOINTS] = {0};
static DHParam zucTaskDHParam = {0};
static RPY conicalCenRpy;
static double deviation;
static int collision_detect_flag = 1;

#define REFIT_NUM 6
static double last_tc_goal_jointpos[ZUCMOT_MAX_JOINTS] = {0};       // for toppra: 上一个tc段目标点的关节位置
static double last_tc_blend_jointpos[ZUCMOT_MAX_JOINTS] = {0};      // for toppra: 上一个tc段转接点的关节位置
static double last_tc_blend_jointpos_sec[ZUCMOT_MAX_JOINTS] = {0};  //for toppra: 上一个tc段转接点下一离散点的关节位置
static double last_tc_refit_jpos[REFIT_NUM][ZUCMOT_MAX_JOINTS] = {0};

static double cur_tc_goal_jointpos[ZUCMOT_MAX_JOINTS] = {0};       //当前tc段目标点的关节位置
static double cur_tc_blend_jointpos[ZUCMOT_MAX_JOINTS] = {0};      //当前tc段转接点的关节位置
static double cur_tc_blend_jointpos_sec[ZUCMOT_MAX_JOINTS] = {0};  //当前tc段转接点下一离散点的关节位置

static double last_tc_blend_r = 0.0;  // for toppra:上一个tc段的转接半径,如果实际运动没有转接成功，需要置零
static double cur_tc_blend_r = 0.0;   // for toppra:当前转接段的转接半径

static double last_final_pathp = 0.0;  //有转接的时候,转接段路径点的值在上一运动段转接点处的路径值开始进行叠加
static int lasttc_jointNum = 0;        //上一个关节运动段运动最长的关节
static int lasttc_left_len = 0;        //上一关节运动段从转接点到目标点离散点的个数
static int first_command = 1;
static int kinematic_error = 0;
static int userset_use_toppra = 0;
static int userset_filter_size = 0;
static int useradd_tor_cons = 1;
static double usertor_portion = 1.0;

static double toolParameters[4] = {0};
static double vf_params[ZUCMOT_MAX_JOINTS] = {0};
static double tor_limit[ZUCMOT_MAX_JOINTS] = {0};
static double install_angle[3] = {0};
static double dh_a[ZUCMOT_MAX_JOINTS] = {0};
static double dh_d[ZUCMOT_MAX_JOINTS] = {0};
static double dh_alpha[ZUCMOT_MAX_JOINTS] = {0};
static double dynParameters[ZUCMOT_MAX_JOINTS * 14] = {0};

static double tcp_vel_limit_toppra = 8000.0;

static BlendSegPath blend_seg_jpos;  //这里存放从转接点到目标点的离散关节位置
#define DISCRETE_JOINT_LENGTH 0.25   // for toppra discrete
#define DISCRETE_CART_LENGTH 1       // for toppra discrete
#define SMALL 1.0e-4
#define MAX_NUM 1.0e8
#define CLOSE_TO_ZERO 1.0e-8
#define MAX_POINT_NUM 1001
#define MAX_BLEND_LEN 1001
#define SS_INCRE 0.1

PyObject* pmodule;
static int unable_use_toppra;
// static TcPathInfo *path_info_queue;
// intermediate identification result
static double intermResWithPayload[4];
static double intermResWithoutPayload[4];

static zucmot_config_t zucmotConfig;
int get_zucmot_debug_info = 1;
static float gripperFreq[MAX_TIO_SIG_NUM] = {0};
double tcpCalError[3] = {0.0};
/* define this to catch isnan errors, for
   rtlinux FPU register problem testing */
#define ISNAN_TRAP

#ifdef ISNAN_TRAP
#define CATCH_NAN(cond)                                    \
    do {                                                   \
        if (cond)                                          \
        {                                                  \
            printf("isnan error in %s()\n", __FUNCTION__); \
            return -1;                                     \
        }                                                  \
    } while (0)
#else
#define CATCH_NAN(cond) \
    do {                \
    } while (0)
#endif

// MOTION INTERFACE
static zucmot_status_t zucmotStatus;
static struct TrajConfig_t TrajConfig;
// static double tcp_vel_limit;
static struct JointConfig_t JointConfig[ZUCMOT_MAX_JOINTS];
static struct AxisConfig_t AxisConfig[ZUCMOT_MAX_AXIS];
static zucmot_command_t zucmotCommand;
static _JK_BL* servo_upgrade;
static int servo_upgrade_id;

__attribute__((unused)) static int zucmotIoInited = 0;  // non-zero means io called init
static int zucmotion_initialized = 0;                   // non-zero means both

static unsigned long localMotionHeartbeat = 0;
static int localMotionCommandType = 0;
static int localMotionEchoSerialNumber = 0;
static int task_init_finish_flag = 0;

// pid of dynamics calculate process
pid_t dynamics_identify_pid = -1;
pid_t friction_identify_pid = -1;

static int kinematicsInverse(const ZucPose* toolOffset,
                             const ZucPose* world,
                             double* joint,
                             const unsigned long* interp_iflags,
                             unsigned long* interp_fflags,
                             const PmRpy* base_offset,
                             const ZucPose* user_offset,
                             uint8_t grp_id = 0)
{
    RobJointVal jpos(joint);
    auto ag = axisgroup::AxisGroupMgr::instance().get_axisgroup(grp_id);
    if (!ag.get())
        return -1;
    auto kine = ag->kine();
    auto config = mot::al::MotionProxy::instance().robot(grp_id)->get_status().robot_config;
    auto tool = axisgroup::RobotToolMgr::get_rob_tool(config.tool_frame_id).pose();
    auto usr = axisgroup::UsrFrameMgr::get_usr_frame(config.user_frame_id).world(config.base_frame_id);

    kine::KineItf::kine_Conf conf;
    int ret = kine->ikine(*world, jpos, conf, &tool, &usr);
    jpos.array_value(joint);
    return ret;
}

static int kinematicsForward(const ZucPose* toolOffset,
                             const double* joint,
                             ZucPose* world,
                             const unsigned long* interp_fflags,
                             unsigned long* interp_iflags,
                             const PmRpy* base_offset,
                             const ZucPose* user_offset,
                             uint8_t grp_id = 0)
{
    int ret = -1;
    RobJointVal jpos(joint);
    ZucPose pose;
    auto ag = axisgroup::AxisGroupMgr::instance().get_axisgroup(grp_id);
    if (!ag.get())
        return -1;
    auto kine = ag->kine();
    auto config = mot::al::MotionProxy::instance().robot(grp_id)->get_status().robot_config;
    auto tool = axisgroup::RobotToolMgr::get_rob_tool(config.tool_frame_id).pose();
    auto usr = axisgroup::UsrFrameMgr::get_usr_frame(config.user_frame_id).world(config.base_frame_id);
    ret = kine->fkine(jpos, pose, &tool, &usr);
    *world = pose;
    return ret;
}

static int checkRobotSafetyParameters(ZUC_ROBOT_SAFETY* robot_safety_param)
{
    if (robot_safety_param->paramType < ZucRobotSafetyType::ROBOT_ALL_PARA || robot_safety_param->paramType > ZucRobotSafetyType::ROBOT_STOPPINGTIME_PARA)
    {
        zucOperatorError(ABNORMAL_VALUES_OF_SAFETY_PARAMETERS, "Abnormal values of safety parameters");
        return -1;
    }

    if (robot_safety_param->custome < ZucRobotSafetyCustom::USER_DEFINED || robot_safety_param->custome > ZucRobotSafetyCustom::STRICTSAFETYSETTING)
    {
        zucOperatorError(ABNORMAL_VALUES_OF_SAFETY_PARAMETERS, "Abnormal values of safety parameters");
        return -1;
    }

    //伟创力定制的手自动模式功能开启，且在自动模式下，禁止设置安全参数 || 正在运动中则禁止设置安全参数
    if ((zucStatus->operation_mode_option == 1 && zucStatus->operation_mode == 1) || zucStatus->motion.traj.inpos != 1)
    {
        zucOperatorError(NONMANUAL_CANNOT_SET_ROBOT_SAFETY_PARAMETERS,
                         "The safety parameters cannot be configured if the robot is not in manual mode or in manual mode motion.");
        return -1;
    }
    //如果程序执行则禁止设置安全参数
    if (zucStatus->task.interpState == ZUC_TASK_INTERP_READING || zucStatus->task.interpState == ZUC_TASK_INTERP_PAUSED ||
        zucStatus->task.interpState == ZUC_TASK_INTERP_WAITING)
    {
        zucOperatorError(PROGRAM_EXECUTE_CANNOT_SET_ROBOT_SAFETY_PARAMETERS, "The safety parameters cannot be configured if the program is being executed.");
        return -1;
    }

    if (zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_ADMITTANCE || zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_DRAG ||
        zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_SERVOJOP)
    {
        zucOperatorError(CANNOT_SET_COLLISION_SENSITIVITY, "cannot set collision sensitivity in drag mode or force control mode");
        return -1;
    }
    rtapi_print("cmd momentumLimit is '%f',  min_momentum = %f, max_momentum = %f \n",
                robot_safety_param->momentumLimit,
                zucStatus->motion.robot_model_data.minMomentum,
                zucStatus->motion.robot_model_data.maxMomentum);
    //动量限制参数检查
    if ((robot_safety_param->momentumLimit - zucStatus->motion.robot_model_data.minMomentum) < -0.0001)
    {
        robot_safety_param->momentumLimit = zucStatus->motion.robot_model_data.minMomentum;
        rtapi_print("robot_serial_num is '%d',  min_momentum = %f, max_momentum = %f \n",
                    zucStatus->motion.robot_serial_num,
                    zucStatus->motion.robot_model_data.minMomentum,
                    zucStatus->motion.robot_model_data.maxMomentum);
        char str[512];
        snprintf(str,
                 512,
                 "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}",
                 zucStatus->motion.robot_model_data.minMomentum,
                 zucStatus->motion.robot_model_data.maxMomentum,
                 "momentumLimit");
        zucOperatorWarning(OUT_OF_RANGE_SET_SAFETY_PARAMETERS, "momentumLimit out of range #%s", str);
    }
    else if ((robot_safety_param->momentumLimit - zucStatus->motion.robot_model_data.maxMomentum) > 0.0001)
    {
        robot_safety_param->momentumLimit = zucStatus->motion.robot_model_data.maxMomentum;
        rtapi_print("robot_serial_num is '%d',  min_momentum = %f, max_momentum = %f \n",
                    zucStatus->motion.robot_serial_num,
                    zucStatus->motion.robot_model_data.minMomentum,
                    zucStatus->motion.robot_model_data.maxMomentum);
        char str[512];
        snprintf(str,
                 512,
                 "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}",
                 zucStatus->motion.robot_model_data.minMomentum,
                 zucStatus->motion.robot_model_data.maxMomentum,
                 "momentumLimit");
        zucOperatorWarning(OUT_OF_RANGE_SET_SAFETY_PARAMETERS, "momentumLimit out of range #%s", str);
    }
    if (robot_safety_param->momentumLimit < zucStatus->reduceSafe.momentum_limit)
    {
        char str[256];
        snprintf(str,
                 256,
                 "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}",
                 robot_safety_param->momentumLimit,
                 zucStatus->reduceSafe.momentum_limit,
                 "momentumLimit");
        zucOperatorError(
            ROBOT_SAFETY_LIMIT_CONFLICT_WITH_REDUCED_SAFETY_LIMIT, "The robot momentum limit conflict with the reduced mode momentum limit. # %s", str);
        return -1;
    }

    //TCP速度限制参数检查
    if ((robot_safety_param->tcpVelLimit - 5000.0) > 0.0001)
    {
        robot_safety_param->tcpVelLimit = 5000.0;
        rtapi_print("robot_serial_num is '%d',  min_tcpVelLimit = %f, max_tcpVelLimit = %f \n", zucStatus->motion.robot_serial_num, 50.0, 5000.0);
        char str[512];
        snprintf(str, 512, "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}", 50.0, 5000.0, "tcpVelLimit");
        zucOperatorWarning(OUT_OF_RANGE_SET_SAFETY_PARAMETERS, "tcpVelLimit out of range #%s", str);
    }
    else if ((robot_safety_param->tcpVelLimit - 50.0) < -0.0001)
    {
        robot_safety_param->tcpVelLimit = 50.0;
        rtapi_print("robot_serial_num is '%d',  min_tcpVelLimit = %f, max_tcpVelLimit = %f \n", zucStatus->motion.robot_serial_num, 50.0, 5000.0);
        char str[512];
        snprintf(str, 512, "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}", 50.0, 5000.0, "tcpVelLimit");
        zucOperatorWarning(OUT_OF_RANGE_SET_SAFETY_PARAMETERS, "tcpVelLimit out of range #%s", str);
    }
    if (robot_safety_param->tcpVelLimit < zucStatus->reduceSafe.tcp_speed)
    {
        char str[256];
        snprintf(str,
                 256,
                 "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}",
                 robot_safety_param->tcpVelLimit,
                 zucStatus->reduceSafe.tcp_speed,
                 "tcpVelLimit");
        zucOperatorError(
            ROBOT_SAFETY_LIMIT_CONFLICT_WITH_REDUCED_SAFETY_LIMIT, "The robot tcp speed limit conflict with the reduced mode tcp speed limit. # %s", str);
        return -1;
    }

    //功率限制参数检查
    if ((robot_safety_param->robotPowerLimit - zucStatus->motion.robot_model_data.minRobotPower) < -0.0001)
    {
        robot_safety_param->robotPowerLimit = zucStatus->motion.robot_model_data.minRobotPower;
        rtapi_print("robot_serial_num is '%d',  min_robotPowerLimit = %f, max_robotPowerLimit = %f \n",
                    zucStatus->motion.robot_serial_num,
                    zucStatus->motion.robot_model_data.minRobotPower,
                    zucStatus->motion.robot_model_data.maxRobotPower);
        char str[512];
        snprintf(str,
                 512,
                 "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}",
                 zucStatus->motion.robot_model_data.minRobotPower,
                 zucStatus->motion.robot_model_data.maxRobotPower,
                 "robotPowerLimit");
        zucOperatorWarning(OUT_OF_RANGE_SET_SAFETY_PARAMETERS, "robotPowerLimit out of range #%s", str);
    }
    else if ((robot_safety_param->robotPowerLimit - zucStatus->motion.robot_model_data.maxRobotPower) > 0.0001)
    {
        robot_safety_param->robotPowerLimit = zucStatus->motion.robot_model_data.maxRobotPower;
        rtapi_print("robot_serial_num is '%d',  min_robotPowerLimit = %f, max_robotPowerLimit = %f \n",
                    zucStatus->motion.robot_serial_num,
                    zucStatus->motion.robot_model_data.minRobotPower,
                    zucStatus->motion.robot_model_data.maxRobotPower);
        char str[512];
        snprintf(str,
                 512,
                 "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}",
                 zucStatus->motion.robot_model_data.minRobotPower,
                 zucStatus->motion.robot_model_data.maxRobotPower,
                 "robotPowerLimit");
        zucOperatorWarning(OUT_OF_RANGE_SET_SAFETY_PARAMETERS, "robotPowerLimit out of range #%s", str);
    }
    if (robot_safety_param->robotPowerLimit < zucStatus->reduceSafe.power_limit)
    {
        char str[256];
        snprintf(str,
                 256,
                 "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}",
                 robot_safety_param->robotPowerLimit,
                 zucStatus->reduceSafe.power_limit,
                 "robotPowerLimit");
        zucOperatorError(ROBOT_SAFETY_LIMIT_CONFLICT_WITH_REDUCED_SAFETY_LIMIT, "The robot power limit conflict with the reduced mode power limit. # %s", str);
        return -1;
    }

    //停止时间参数检查
    if ((robot_safety_param->stoppingTime - 5.0) > 0.0001)
    {
        robot_safety_param->stoppingTime = 5.0;
        rtapi_print("robot_serial_num is '%d',  min_stoppingTime = %f, max_stoppingTime = %f \n", zucStatus->motion.robot_serial_num, 0.1, 5.0);
        char str[512];
        snprintf(str, 512, "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}", 0.1, 5.0, "stoppingTime");
        zucOperatorWarning(OUT_OF_RANGE_SET_SAFETY_PARAMETERS, "stoppingTime out of range #%s", str);
    }
    else if ((robot_safety_param->stoppingTime - 0.1) < -0.0001)
    {
        robot_safety_param->stoppingTime = 0.1;
        rtapi_print("robot_serial_num is '%d',  min_stoppingTime = %f, max_stoppingTime = %f \n", zucStatus->motion.robot_serial_num, 0.1, 5.0);
        char str[512];
        snprintf(str, 512, "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}", 0.1, 5.0, "stoppingTime");
        zucOperatorWarning(OUT_OF_RANGE_SET_SAFETY_PARAMETERS, "stoppingTime out of range #%s", str);
    }
    if (robot_safety_param->stoppingTime < zucStatus->reduceSafe.stopping_time)
    {
        char str[256];
        snprintf(str,
                 256,
                 "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}",
                 robot_safety_param->stoppingTime,
                 zucStatus->reduceSafe.stopping_time,
                 "stoppingTime");
        zucOperatorError(
            ROBOT_SAFETY_LIMIT_CONFLICT_WITH_REDUCED_SAFETY_LIMIT, "The robot stoppingTime conflict with the reduced mode stoppingTime. # %s", str);
        return -1;
    }

    // 停止距离参数检查
    if ((robot_safety_param->stoppingDistance - 4000.0) > 0.0001)
    {
        robot_safety_param->stoppingDistance = 4000.0;
        rtapi_print("robot_serial_num is '%d',  min_stoppingDistance = %f, max_stoppingDistance = %f \n", zucStatus->motion.robot_serial_num, 100.0, 4000.0);
        char str[512];
        snprintf(str, 512, "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}", 0.1, 5.0, "stoppingDistance");
        zucOperatorWarning(OUT_OF_RANGE_SET_SAFETY_PARAMETERS, "stoppingDistance out of range #%s", str);
    }
    else if ((robot_safety_param->stoppingDistance - 100.0) < -0.0001)
    {
        zucStatus->robotSafe[0].stoppingDistance = 100.0;
        rtapi_print("robot_serial_num is '%d',  min_stoppingDistance = %f, max_stoppingDistance = %f \n", zucStatus->motion.robot_serial_num, 100.0, 4000.0);
        char str[512];
        snprintf(str, 512, "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}", 0.1, 5.0, "stoppingDistance");
        zucOperatorWarning(OUT_OF_RANGE_SET_SAFETY_PARAMETERS, "stoppingDistance out of range #%s", str);
    }
    if (robot_safety_param->stoppingDistance < zucStatus->reduceSafe.stopping_dist)
    {
        char str[256];
        snprintf(str,
                 256,
                 "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}",
                 robot_safety_param->stoppingDistance,
                 zucStatus->reduceSafe.stopping_dist,
                 "stoppingDistance");
        zucOperatorError(
            ROBOT_SAFETY_LIMIT_CONFLICT_WITH_REDUCED_SAFETY_LIMIT, "The robot stoppingDistance conflict with the reduced mode stoppingDistance. # %s", str);
        return -1;
    }

    return 0;
}

static int checkReduceSafetyParameters(reduce_param_config* reduce_safety_param)
{
    //伟创力定制的手自动模式功能开启，且在自动模式下，禁止设置安全参数 || 正在运动中则禁止设置安全参数
    if ((zucStatus->operation_mode_option == 1 && zucStatus->operation_mode == 1) || zucStatus->motion.traj.inpos != 1)
    {
        zucOperatorError(NONMANUAL_CANNOT_SET_ROBOT_SAFETY_PARAMETERS,
                         "The safety parameters cannot be configured if the robot is not in manual mode or in manual mode motion.");
        return -1;
    }
    //如果程序执行则禁止设置安全参数
    if (zucStatus->task.interpState == ZUC_TASK_INTERP_READING || zucStatus->task.interpState == ZUC_TASK_INTERP_PAUSED ||
        zucStatus->task.interpState == ZUC_TASK_INTERP_WAITING)
    {
        zucOperatorError(PROGRAM_EXECUTE_CANNOT_SET_ROBOT_SAFETY_PARAMETERS, "The safety parameters cannot be configured if the program is being executed.");
        return -1;
    }

    //缩减模式TCP速度限制参数检查
    if ((reduce_safety_param->tcp_vel_reduce - 250.0) > 0.0001)
    {
        reduce_safety_param->tcp_vel_reduce = 250.0;
        rtapi_print("robot_serial_num is '%d',  min_tcp_vel_reduce = %f, max_tcp_vel_reduce = %f \n", zucStatus->motion.robot_serial_num, 50.0, 250.0);
        char str[512];
        snprintf(str, 512, "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}", 50.0, 250.0, "tcp_vel_reduce");
        zucOperatorWarning(OUT_OF_RANGE_SET_SAFETY_PARAMETERS, "tcpVelLimit out of range #%s", str);
    }
    else if ((reduce_safety_param->tcp_vel_reduce - 50.0) < -0.0001)
    {
        reduce_safety_param->tcp_vel_reduce = 50.0;
        rtapi_print("robot_serial_num is '%d',  min_tcp_vel_reduce = %f, max_tcp_vel_reduce = %f \n", zucStatus->motion.robot_serial_num, 50.0, 250.0);
        char str[512];
        snprintf(str, 512, "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}", 50.0, 250.0, "tcp_vel_reduce");
        zucOperatorWarning(OUT_OF_RANGE_SET_SAFETY_PARAMETERS, "tcpVelLimit out of range #%s", str);
    }
    if (reduce_safety_param->tcp_vel_reduce > zucStatus->robotSafe[0].tcpVelLimit)
    {
        char str[256];
        snprintf(str,
                 256,
                 "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}",
                 zucStatus->robotSafe[0].tcpVelLimit,
                 reduce_safety_param->tcp_vel_reduce,
                 "tcp_vel_reduce");
        zucOperatorError(
            ROBOT_SAFETY_LIMIT_CONFLICT_WITH_REDUCED_SAFETY_LIMIT, "The robot tcp speed limit conflict with the reduced mode tcp speed limit. # %s", str);
        return -1;
    }

    //缩减模式肘部速度限制参数检查
    if ((reduce_safety_param->elbow_vel_limit_reduce - 250.0) > 0.0001)
    {
        reduce_safety_param->elbow_vel_limit_reduce = 250.0;
        rtapi_print(
            "robot_serial_num is '%d',  min_elbow_vel_limit_reduce = %f, max_elbow_vel_limit_reduce = %f \n", zucStatus->motion.robot_serial_num, 50.0, 250.0);
        char str[512];
        snprintf(str, 512, "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}", 50.0, 250.0, "elbow_vel_limit_reduce");
        zucOperatorWarning(OUT_OF_RANGE_SET_SAFETY_PARAMETERS, "elbow_vel_limit_reduce out of range #%s", str);
    }
    else if ((reduce_safety_param->elbow_vel_limit_reduce - 50.0) < -0.0001)
    {
        reduce_safety_param->elbow_vel_limit_reduce = 50.0;
        rtapi_print(
            "robot_serial_num is '%d',  min_elbow_vel_limit_reduce = %f, max_elbow_vel_limit_reduce = %f \n", zucStatus->motion.robot_serial_num, 50.0, 250.0);
        char str[512];
        snprintf(str, 512, "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}", 50.0, 250.0, "elbow_vel_limit_reduce");
        zucOperatorWarning(OUT_OF_RANGE_SET_SAFETY_PARAMETERS, "elbow_vel_limit_reduce out of range #%s", str);
    }

    //缩减模式动量限制参数检查
    if ((reduce_safety_param->robot_momentum_limit_reduce - zucStatus->motion.robot_model_data.minReduceMomentum) < -0.0001)
    {
        reduce_safety_param->robot_momentum_limit_reduce = zucStatus->motion.robot_model_data.minReduceMomentum;
        rtapi_print("robot_serial_num is '%d',  min_robot_momentum_limit_reduce = %f, max_robot_momentum_limit_reduce = %f \n",
                    zucStatus->motion.robot_serial_num,
                    zucStatus->motion.robot_model_data.minReduceMomentum,
                    zucStatus->motion.robot_model_data.maxReduceMomentum);
        char str[512];
        snprintf(str,
                 512,
                 "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}",
                 zucStatus->motion.robot_model_data.minReduceMomentum,
                 zucStatus->motion.robot_model_data.maxReduceMomentum,
                 "robot_momentum_limit_reduce");
        zucOperatorWarning(OUT_OF_RANGE_SET_SAFETY_PARAMETERS, "robot_momentum_limit_reduce out of range #%s", str);
    }
    else if ((reduce_safety_param->robot_momentum_limit_reduce - zucStatus->motion.robot_model_data.maxReduceMomentum) > 0.0001)
    {
        reduce_safety_param->robot_momentum_limit_reduce = zucStatus->motion.robot_model_data.maxReduceMomentum;
        rtapi_print("robot_momentum_limit_reduce is '%f',  min_robot_momentum_limit_reduce = %f, max_robot_momentum_limit_reduce = %f \n",
                    reduce_safety_param->robot_momentum_limit_reduce,
                    zucStatus->motion.robot_model_data.minReduceMomentum,
                    zucStatus->motion.robot_model_data.maxReduceMomentum);
        char str[512];
        snprintf(str,
                 512,
                 "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}",
                 zucStatus->motion.robot_model_data.minReduceMomentum,
                 zucStatus->motion.robot_model_data.maxReduceMomentum,
                 "robot_momentum_limit_reduce");
        zucOperatorWarning(OUT_OF_RANGE_SET_SAFETY_PARAMETERS, "robot_momentum_limit_reduce out of range #%s", str);
    }
    if (reduce_safety_param->robot_momentum_limit_reduce > zucStatus->robotSafe[0].momentumLimit)
    {
        char str[256];
        snprintf(str,
                 256,
                 "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}",
                 zucStatus->robotSafe[0].momentumLimit,
                 reduce_safety_param->robot_momentum_limit_reduce,
                 "robot_momentum_limit_reduce");
        zucOperatorError(
            ROBOT_SAFETY_LIMIT_CONFLICT_WITH_REDUCED_SAFETY_LIMIT, "The robot momentum limit conflict with the reduced mode momentum limit. # %s", str);
        return -1;
    }

    //缩减模式功率限制参数检查
    if ((reduce_safety_param->robot_poewr_limit_reduce - zucStatus->motion.robot_model_data.minReduceRobotPower) < -0.0001)
    {
        reduce_safety_param->robot_poewr_limit_reduce = zucStatus->motion.robot_model_data.minReduceRobotPower;
        rtapi_print("robot_poewr_limit_reduce is '%f',  min_robot_poewr_limit_reduce = %f, max_robot_poewr_limit_reduce = %f \n",
                    reduce_safety_param->robot_poewr_limit_reduce,
                    zucStatus->motion.robot_model_data.minReduceRobotPower,
                    zucStatus->motion.robot_model_data.maxReduceRobotPower);
        char str[512];
        snprintf(str,
                 512,
                 "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}",
                 zucStatus->motion.robot_model_data.minReduceRobotPower,
                 zucStatus->motion.robot_model_data.maxReduceRobotPower,
                 "robot_poewr_limit_reduce");
        zucOperatorWarning(OUT_OF_RANGE_SET_SAFETY_PARAMETERS, "robot_poewr_limit_reduce out of range #%s", str);
    }
    else if ((reduce_safety_param->robot_poewr_limit_reduce - zucStatus->motion.robot_model_data.maxReduceRobotPower) > 0.0001)
    {
        reduce_safety_param->robot_poewr_limit_reduce = zucStatus->motion.robot_model_data.maxReduceRobotPower;
        rtapi_print("robot_poewr_limit_reduce is '%f',  min_robot_poewr_limit_reduce = %f, max_robot_poewr_limit_reduce = %f \n",
                    reduce_safety_param->robot_poewr_limit_reduce,
                    zucStatus->motion.robot_model_data.minReduceRobotPower,
                    zucStatus->motion.robot_model_data.maxReduceRobotPower);
        char str[512];
        snprintf(str,
                 512,
                 "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}",
                 zucStatus->motion.robot_model_data.minReduceRobotPower,
                 zucStatus->motion.robot_model_data.maxReduceRobotPower,
                 "robot_poewr_limit_reduce");
        zucOperatorWarning(OUT_OF_RANGE_SET_SAFETY_PARAMETERS, "robot_poewr_limit_reduce out of range #%s", str);
    }
    if (reduce_safety_param->robot_poewr_limit_reduce > zucStatus->robotSafe[0].robotPowerLimit)
    {
        char str[256];
        snprintf(str,
                 256,
                 "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}",
                 zucStatus->robotSafe[0].robotPowerLimit,
                 reduce_safety_param->robot_poewr_limit_reduce,
                 "robot_poewr_limit_reduce");
        zucOperatorError(ROBOT_SAFETY_LIMIT_CONFLICT_WITH_REDUCED_SAFETY_LIMIT, "The robot power limit conflict with the reduced mode power limit. # %s", str);
        return -1;
    }

    //缩减模式停止时间参数限制
    if ((reduce_safety_param->stopping_time_reduce - 0.015) < -0.0001)
    {
        reduce_safety_param->stopping_time_reduce = 0.015;
        rtapi_print("stopping_time_reduce is '%f',  min_stopping_time_reduce = %f, max_stopping_time_reduce = %f \n",
                    reduce_safety_param->stopping_time_reduce,
                    0.015,
                    5.0);
        char str[512];
        snprintf(str, 512, "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}", 0.015, 5.0, "stopping_time_reduce");
        zucOperatorWarning(OUT_OF_RANGE_SET_SAFETY_PARAMETERS, "stopping_time_reduce out of range #%s", str);
    }
    else if ((reduce_safety_param->stopping_time_reduce - 5.0) > 0.0001)
    {
        reduce_safety_param->stopping_time_reduce = 5.0;
        rtapi_print("rstopping_time_reduce is '%f',  min_stopping_time_reduce = %f, max_stopping_time_reduce = %f \n",
                    reduce_safety_param->stopping_time_reduce,
                    0.015,
                    5.0);
        char str[512];
        snprintf(str, 512, "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}", 0.015, 5.0, "stopping_time_reduce");
        zucOperatorWarning(OUT_OF_RANGE_SET_SAFETY_PARAMETERS, "stopping_time_reduce out of range #%s", str);
    }
    if (reduce_safety_param->stopping_time_reduce > zucStatus->robotSafe[0].stoppingTime)
    {
        char str[256];
        snprintf(str,
                 256,
                 "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}",
                 zucStatus->robotSafe[0].stoppingTime,
                 reduce_safety_param->stopping_time_reduce,
                 "stopping_time_reduce");
        zucOperatorError(ROBOT_SAFETY_LIMIT_CONFLICT_WITH_REDUCED_SAFETY_LIMIT,
                         "The robot stopping time limit conflict with the reduced mode stopping time limit. # %s",
                         str);
        return -1;
    }

    //缩减模式停止距离参数限制
    if ((reduce_safety_param->stopping_dist_reduce - 0.35) < -0.0001)
    {
        reduce_safety_param->stopping_dist_reduce = 0.35;
        rtapi_print("stopping_dist_reduce is '%f',  min_stopping_dist_reduce = %f, max_stopping_dist_reduce = %f \n",
                    reduce_safety_param->stopping_dist_reduce,
                    0.35,
                    4000.0);
        char str[512];
        snprintf(str, 512, "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}", 0.35, 4000.0, "stopping_dist_reduce");
        zucOperatorWarning(OUT_OF_RANGE_SET_SAFETY_PARAMETERS, "stopping_dist_reduce out of range #%s", str);
    }
    else if (reduce_safety_param->stopping_dist_reduce - 4000.0 > 0.0001)
    {
        reduce_safety_param->stopping_dist_reduce = 4000.0;
        rtapi_print("stopping_dist_reduce is '%f',  min_stopping_dist_reduce = %f, max_stopping_dist_reduce = %f \n",
                    reduce_safety_param->stopping_dist_reduce,
                    0.35,
                    4000.0);
        char str[512];
        snprintf(str, 512, "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}", 0.35, 4000.0, "stopping_dist_reduce");
        zucOperatorWarning(OUT_OF_RANGE_SET_SAFETY_PARAMETERS, "stopping_dist_reduce out of range #%s", str);
    }
    if (reduce_safety_param->stopping_dist_reduce > zucStatus->robotSafe[0].stoppingDistance)
    {
        char str[256];
        snprintf(str,
                 256,
                 "{\"fixed_key\":[\"%.3f\",\"%.3f\"],\"param\":{\"0\":\"%s\"}}",
                 zucStatus->robotSafe[0].stoppingDistance,
                 reduce_safety_param->stopping_dist_reduce,
                 "stopping_dist_reduce");
        zucOperatorError(ROBOT_SAFETY_LIMIT_CONFLICT_WITH_REDUCED_SAFETY_LIMIT,
                         "The robot stopping time limit conflict with the reduced mode stopping time limit. # %s",
                         str);
        return -1;
    }

    return 0;
}

static int syncFileToDisk(const char* filename)
{
    int fd;
    if (filename == NULL || (fd = open(filename, O_RDWR | O_CREAT)) == -1)
    {
        return -1;
    }

    fsync(fd);
    close(fd);
    return 0;
}

int zucGetModelFromSerialNum(unsigned int serialNum, int* robotModel)
{
    if (robotModel == nullptr)
        return -1;

    *robotModel = MODEL_UNSUPPORTED;
    if (serialNum > 710000 && serialNum < 710999)
    {
        *robotModel = MODEL_ZU_710;
    }
    else if (serialNum > 0)
    {
        *robotModel = serialNum / 10000;
    }
    return 0;
}

int zucSetSafeZoneEnableOpt(int openEna, int autoRunEna)
{
    TrajConfig.safeZoneOpenEnable = openEna;
    TrajConfig.safeZoneAutoEnable = autoRunEna;
    return zucUpdateSafeZoneEna();
}

int zucUpdateSafeZoneEna()
{
    int enable;
    if (TrajConfig.safeZoneOpenEnable)
    {
        enable = 1;
    }
    else if (TrajConfig.safeZoneAutoEnable && zucStatus->task.interpState != ZUC_TASK_INTERP_IDLE)
    {
        enable = 1;
    }
    else
    {
        enable = 0;
    }

    auto cmd = std::make_shared<CmdSetSafeZoneEnable>(0, enable);
    return mot::realtime_cmd_call(cmd);
}

int zucSetSafeZone(SafeZone zone)
{
    int ret;

    if (task_init_finish_flag)
    {
        usrmotReadZucmotStatus(&zucmotStatus);
        iniChecksumPersistence(zucmotStatus.safetyCRC, CRC_FILE);
    }
    auto cmd = std::make_shared<CmdSetSafeZone>(0, zone);
    return mot::realtime_cmd_call(cmd);
}

int zucSetSafeZoneElbowWristSphereOpt(double SafezoneElbowRadius, double SafezoneElbowOffest)
{
    TrajConfig.SafezoneElbowRadius = SafezoneElbowRadius;
    TrajConfig.SafezoneElbowOffest = SafezoneElbowOffest;
    return zucUpdateSafeZoneElbowWristSphere();
}

int zucUpdateSafeZoneElbowWristSphere()
{
    auto cmd = std::make_shared<CmdSetSafeZoneElbowWristSphere>(0, TrajConfig.SafezoneElbowOffest, TrajConfig.SafezoneElbowRadius);
    return mot::realtime_cmd_call(cmd);
}

int zucStartPayloadIdentifyTraj(int trajId, JointTraj jointTraj, int withPayload)
{
    if (fabs(install_angle[0]) > Q_FUZZ || fabs(install_angle[1]) > Q_FUZZ || fabs(install_angle[2]) > Q_FUZZ)
    {
        zucOperatorError(EEROR_START_PAYLOAD_IDENTIFY, "Can't Identify Payload when not in floor-mounting");
        return -1;
    }

    auto cmd = std::make_shared<CmdStartPayloadIdentify>(0, withPayload, jointTraj, trajId);
    return mot::realtime_cmd_call(cmd);
}

int zucResetPayloadIdentifyStatus(int userOpt)
{
    if (userOpt == 0)
    {  // all
        for (int i = 0; i < 3; i++)
        {
            zucStatus->motion.onloadIdentifyStat[i] = 0;
            zucStatus->motion.noloadIdentifyStat[i] = 0;
        }
    }
    else
    {  // only with load
        for (int i = 0; i < 3; i++) { zucStatus->motion.onloadIdentifyStat[i] = 0; }
    }
    return 0;
}

int zucGetPayloadIdentifyResult()
{
    bool identify_complete = true;
    for (int i = 0; i < 3; i++) { identify_complete = identify_complete && (zucStatus->motion.onloadIdentifyStat[i] == 2); }

    if (!identify_complete)
    {
        usrOpeErrCode = -1;
        return -1;  // identification not complete
    }

    // @FIXME liwang 此处不适用于7轴机器人
    double linklen[3] = {zucTaskDHParam.d[3], zucTaskDHParam.d[4], zucTaskDHParam.d[5]};
    if (calpayload(linklen, &(zucStatus->motion.utilPayload.m), zucStatus->motion.utilPayload.p) == -1)
    {
        zucStatus->motion.utilPayload.m = 0;
        zucStatus->motion.utilPayload.p[0] = 0;
        zucStatus->motion.utilPayload.p[1] = 1;
        zucStatus->motion.utilPayload.p[2] = 2;

        usrOpeErrCode = -1;
        return -1;
    }
    usrOpeErrCode = 0;
    // rcs_print("identification result m=%f\n", zucStatus->motion.utilPayload.m);
    // rcs_print("identification result x=%f\n", zucStatus->motion.utilPayload.p[0]);
    // rcs_print("identification result y=%f\n", zucStatus->motion.utilPayload.p[1]);
    // rcs_print("identification result z=%f\n", zucStatus->motion.utilPayload.p[2]);
    return 0;
}

int zucStartIdentifyFriction(int robot_id, int friction_identify_joint_index)
{
    auto cmd = std::make_shared<CmdStartIdentifyFriction>(robot_id, friction_identify_joint_index);
    return mot::realtime_cmd_call(cmd);
}

int zucInFrictionIdentifyCalculating()
{
    auto cmd = std::make_shared<CmdInFrictionIdentifyCalculating>(0);
    return mot::realtime_cmd_call(cmd);
}

int zucCompleteFrictionIdentify()
{
    auto cmd = std::make_shared<CmdCompleteFrictionIdentify>(0);
    return mot::realtime_cmd_call(cmd);
}

int zucSetServoFriction(int robot_id, const double* friction_para, short jnt_id)
{
    int para[128] = {};
    // @FIXME liwang 动力学参数的适配
    char file_name[128];
    sprintf(file_name, "%s_%d.txt", Friction_RESULT_FILE, jnt_id);

    FILE* fp = fopen(file_name, "r");
    if (!fp)
    {
        rtapi_print("Identify file miss, %s!\n", file_name);
        return 0;
    }
    double temp = 0;
    for (int i = 0; i < 6; i++) { fscanf(fp, "%lf", &temp); }  // 第一行6个数据略过
    for (int i = 0; i < 60; i++)
    {
        fscanf(fp, "%lf", &temp);
        para[i] = (int)(temp * 100000);
    };
    auto cmd = std::make_shared<CmdSetServoDynamicsPara>(robot_id, para);
    return mot::realtime_cmd_call(cmd);
}

int zucGetRs485Message(int robot_id, int chn, int len)
{
    auto cmd = std::make_shared<CmdGetRs485Message>(robot_id, len, chn);
    return mot::realtime_cmd_call(cmd);
}

int zucSetTorqueControlEnable(int torque_control_enable, int torque_last_period)
{
    auto cmd = std::make_shared<CmdSetTorqueControlEnable>(0, torque_last_period, torque_control_enable);
    return mot::realtime_cmd_call(cmd);
}

int zucStartDynamicsIdentify()
{
    auto cmd = std::make_shared<DynCmdStartDynamicsIdentify>(0);
    return mot::realtime_cmd_call(cmd);
}

int zucStopDynamicsIdentify()
{
    auto cmd = std::make_shared<DynCmdStopDynamicsIdentify>(0);
    return mot::realtime_cmd_call(cmd);
}

int zucResetDynamicsIdentify()
{
    auto cmd = std::make_shared<DynCmdResetDynamicsIdentify>(0);
    return mot::realtime_cmd_call(cmd);
}

int zucInDynamicsIdentifyCalculating()
{
    auto cmd = std::make_shared<DynCmdInCalcDynamicsIdentify>(0);
    return mot::realtime_cmd_call(cmd);
}

int zucCompleteDynamicsIdentify()
{
    auto cmd = std::make_shared<DynCmdCompleteDynamicsIdentify>(0);
    return mot::realtime_cmd_call(cmd);
}

int zucSetDynamicsIdentifyTraj(const double* traj)
{
    auto cmd = std::make_shared<DynCmdSetDynamicsIdentifyTraj>(0, traj);
    return mot::realtime_cmd_call(cmd);
}

int zucGetDynamicsIdentifyResult()
{
    // if (zucmotStatus.dynamics_identifying_status == 1)
    // {
    //     return 0;
    // }
    // if (dynamics_identify_pid == -1 && zucmotStatus.dynamics_identifying_status == 2)
    // {
    //     zucTrajSetMode(ZUCMOT_MOTION_FREE);  // 恢复Free模式
    //     zucmotStatus.dynamics_identifying_status = 3;
    //     zucInDynamicsIdentifyCalculating();
    //     dynamics_identify_pid = fork();
    //     if (dynamics_identify_pid != -1)
    //     {
    //         rcs_print("Create child process for calculating, %d\n", dynamics_identify_pid);
    //     }
    // }
    // if (dynamics_identify_pid == 0)
    // {
    //     // child
    //     rcs_print("child do something...PID:%d, PPID:%d\n", getpid(), getppid());
    //     int res = 0;
    //     double a[ZUCMOT_MAX_JOINTS] = {0.0};
    //     double d[ZUCMOT_MAX_JOINTS] = {0.0};
    //     double alpha[ZUCMOT_MAX_JOINTS] = {0.0};
    //     double offset[ZUCMOT_MAX_JOINTS] = {0.0};
    //     for (int i = 0; i < zucStatus->motion.robot_joint_num; i++)
    //     {
    //         a[i] = zucTaskDHParam.a[i] / 1000.0;
    //         d[i] = zucTaskDHParam.d[i] / 1000.0;
    //         alpha[i] = zucTaskDHParam.alpha[i] / 180.0 * M_PI;
    //         offset[i] = zucTaskDHParam.joint_homeoff[i] / 180.0 * M_PI;
    //         // #TODO 这里的offset只是零位偏置，实际上还有关节偏置
    //         // #TODO 侧装辨识
    //         rcs_print("a : %lf, d : %lf, alpha : %lf, offset : %lf\n", a[i], d[i], alpha[i], offset[i]);
    //     }
    //     double grav[3] = {0.0, 0.0, -9.8};  // FIXME should be the same with setup
    //     double pi_full[14 * ZUCMOT_MAX_JOINTS] = {0.0};

    //     if (zucStatus->motion.robot_serial_num / 100000000 == 0)
    //     {
    //         zuIdentify(a, d, alpha, offset, grav, pi_full, "/tmp/dynamics_identify_file.txt", DYNAMIC_RESULT_FILE);
    //     }
    //     else if (zucStatus->motion.robot_serial_num / 100000000 == 2)
    //     {
    //         // 动力学使用的是MDH建模，这里alpha角使用MDH角
    //         alpha[0] = 0;
    //         alpha[1] = PM_PI / 2;
    //         alpha[2] = -PM_PI / 2;
    //         alpha[3] = PM_PI / 2;
    //         alpha[4] = -PM_PI / 2;
    //         alpha[5] = PM_PI / 2;
    //         alpha[6] = -PM_PI / 2;
    //         AtIdentify(a, d, alpha, offset, grav, pi_full, "/tmp/dynamics_identify_file.txt", DYNAMIC_RESULT_FILE);
    //     }
    //     exit(0);
    // }
    return 0;
}

int zucGetFrictionIdentifyResult()
{
    // @FIXME liwang
    // if (zucmotStatus.friction_identifying_status == 1)
    // {
    //     return 0;
    // }
    // if (friction_identify_pid == -1 && zucmotStatus.friction_identifying_status == 2)
    // {
    //     zucmotStatus.friction_identifying_status = 3;
    //     zucInFrictionIdentifyCalculating();
    //     friction_identify_pid = fork();
    //     if (friction_identify_pid != -1)
    //     {
    //         rcs_print("Create child process for calculating, %d\n", friction_identify_pid);
    //     }
    // }
    // if (friction_identify_pid == 0)
    // {
    //     // child
    //     rcs_print("child do something...PID:%d, PPID:%d\n", getpid(), getppid());
    //     int res = 0;
    //     double a[ZUCMOT_MAX_JOINTS] = {0.0};
    //     double d[ZUCMOT_MAX_JOINTS] = {0.0};
    //     double alpha[ZUCMOT_MAX_JOINTS] = {0.0};
    //     double offset[ZUCMOT_MAX_JOINTS] = {0.0};
    //     for (int i = 0; i < zucStatus->motion.robot_joint_num; i++)
    //     {
    //         a[i] = zucTaskDHParam.a[i] / 1000.0;
    //         d[i] = zucTaskDHParam.d[i] / 1000.0;
    //         alpha[i] = zucTaskDHParam.alpha[i] / 180.0 * M_PI;
    //         offset[i] = zucTaskDHParam.offset[i] / 180.0 * M_PI;
    //         rcs_print("a : %lf, d : %lf, alpha : %lf, offset : %lf\n", a[i], d[i], alpha[i], offset[i]);
    //     }
    //     double grav[3] = {0.0, 0.0, -9.8};  // FIXME should be the same with setup
    //     double pi_full[14 * ZUCMOT_MAX_JOINTS] = {0.0};
    //     if ()
    //         IdentifyOnline::ZuIdentifyDynamics identify_robot(a, d, alpha, grav, pi_full);
    //     IdentifyOnline::IdentifyResult result;
    //     char file_name[128];
    //     sprintf(file_name, "%s_%d.txt", "/tmp/friction_identify_file", zucStatus->motion.friction_identify_joint_index);
    //     if (!identify_robot.IdentifyFriction(file_name, IdentifyOnline::IdentifyDynamics::TRADITIONAL, zucStatus->motion.friction_identify_joint_index, result))
    //     {
    //         rcs_print("Identify friction Error!\n");  // FXIME shoudl report error to APP
    //         res = -1;
    //     }
    //     else
    //     {
    //         rcs_print("Identify Result:\n");
    //         for (int i = 0; i < zucStatus->motion.robot_joint_num; i++)
    //         { rcs_print("max : %lf, mean : %lf, std : %lf\n", result.err_max[i], result.err_mean[i], result.err_std[i]); }
    //         // 使用原有的侧装参数
    //         char file_name[128];
    //         sprintf(file_name, "%s_%d.txt", Friction_RESULT_FILE, zucStatus->motion.friction_identify_joint_index);

    //         identify_robot.RecordResult(
    //             file_name, zucStatus->motion.robot_model_data.dynParam.kineticK, zucStatus->motion.friction_identify_joint_index, result);

    //         // for (int i = 0; i < 60; i++) { utilDynamics.friction_dynamics[i] = zucStatus->motion.robot_model_data.dynParam.kineticK[i]; }
    //         // // 复制摩擦力参数
    //         // utilDynamics.friction_dynamics[42 + zucStatus->motion.friction_identify_joint_index] = result.pi_star[0];
    //         // utilDynamics.friction_dynamics[48 + zucStatus->motion.friction_identify_joint_index] = result.pi_star[1];
    //         // utilDynamics.friction_dynamics[54 + zucStatus->motion.friction_identify_joint_index] = result.pi_star[2];
    //     }
    //     exit(0);
    // }
    return 0;
}

static int zucInitAutoOp(const char* inifile)
{
    if (inifile == NULL)
        return -1;
    Util::IniParser ini_file(inifile);
    // ZucIniFile trajInifile;
    // trajInifile.Open(inifile);

    // trajInifile.EnableExceptions(ZucIniFile::ERR_CONVERSION);
    try
    {
        int autoenable = 0, autoprogram = 0;
        ini_file.get("TASK.AUTO_ENABLE", (bool&)autoenable, false);
        ini_file.get("TASK.AUTO_PROGRAM", (bool&)autoprogram, false);
        // trajInifile.Find(&autoenable, "AUTO_ENABLE", "TASK");
        zucStatus->autoopsetting.onekeyenable = autoenable;
        // trajInifile.Find(&autoprogram, "AUTO_PROGRAM", "TASK");
        zucStatus->autoopsetting.autoprogram = autoprogram;
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

static int zucInitFuncIOMap(const char* inifile)
{
    if (inifile == NULL)
        return -1;

    // IniFile ini;
    Util::IniParser ini(inifile);
    FuncIOMap funcIOMap;
    funcIOMap.task_run_di = {-1, -1};
    funcIOMap.task_resume_di = {-1, -1};
    funcIOMap.task_pause_di = {-1, -1};
    funcIOMap.task_stop_di = {-1, -1};
    funcIOMap.percentage_mode_level1_di = {-1, -1};
    funcIOMap.stop_mode_di = {-1, -1};
    funcIOMap.percentage_mode_level2_di = {-1, -1};
    funcIOMap.clear_error_di = {-1, -1};
    funcIOMap.drag_mode_di = {-1, -1};
    funcIOMap.drag_mode_disable_di = {-1, -1};
    for (int i = 0; i < 4; ++i)
    {
        funcIOMap.task_idle_do[i] = {-1, -1};
        funcIOMap.task_running_do[i] = {-1, -1};
        funcIOMap.task_paused_do[i] = {-1, -1};
        funcIOMap.stat_fault_do[i] = {-1, -1};
        funcIOMap.stat_poweron_do[i] = {-1, -1};
        funcIOMap.stat_enabled_do[i] = {-1, -1};
        funcIOMap.stat_motion_do[i] = {-1, -1};
        funcIOMap.stat_static_do[i] = {-1, -1};
        funcIOMap.stat_boot_do[i] = {-1, -1};
        funcIOMap.stat_emergency_stop_do[i] = {-1, -1};
        funcIOMap.stat_percentage_mode_level2_do[i] = {-1, -1};
        funcIOMap.stat_sys_protective_stop_do[i] = {-1, -1};
        funcIOMap.stat_initial_pose_do[i] = {-1, -1};
        funcIOMap.stat_percentage_mode_level1_do[i] = {-1, -1};
        funcIOMap.stat_drag_mode_do[i] = {-1, -1};
        funcIOMap.stat_collision_mode_do[i] = {-1, -1};
        funcIOMap.stat_manual_mode_do[i] = {-1, -1};
        funcIOMap.stat_auto_mode_do[i] = {-1, -1};
    }

    //ini.Open(inifile);
    try
    {
        std::string strOptList;
        std::vector<std::string> optList;
        std::vector<std::string> itemList;

        //const char* runOptList = ini.Find("TASK_RUN_MAP_DI", "FUNCTIONIO");
        ini.get("FUNCTIONIO.TASK_RUN_MAP_DI", strOptList, "{-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(":"));
        // optList = split(strOptList, ':');
        funcIOMap.task_run_di.type = std::stoi(optList[0]);
        funcIOMap.task_run_di.index = std::stoi(optList[1]);
        // rcs_print("task_run_di: %d %d\n", funcIOMap.task_run_di.type,funcIOMap.task_run_di.index);

        //const char* pauseOptList = ini.Find("TASK_PAUSE_MAP_DI", "FUNCTIONIO");
        ini.get("FUNCTIONIO.TASK_PAUSE_MAP_DI", strOptList, "{-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        //optList = split(strOptList, ':');
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(":"));
        funcIOMap.task_pause_di.type = std::stoi(optList[0]);
        funcIOMap.task_pause_di.index = std::stoi(optList[1]);
        // rcs_print("task_pause_di: %d %d\n", funcIOMap.task_pause_di.type,funcIOMap.task_pause_di.index);

        //const char* resumeOptList = ini.Find("TASK_RESUME_MAP_DI", "FUNCTIONIO");
        ini.get("FUNCTIONIO.TASK_RESUME_MAP_DI", strOptList, "{-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        //optList = split(strOptList, ':');
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(":"));
        funcIOMap.task_resume_di.type = std::stoi(optList[0]);
        funcIOMap.task_resume_di.index = std::stoi(optList[1]);
        // rcs_print("task_resume_di: %d %d\n", funcIOMap.task_resume_di.type,funcIOMap.task_resume_di.index);

        //const char* stopOptList = ini.Find("TASK_STOP_MAP_DI", "FUNCTIONIO");
        ini.get("FUNCTIONIO.TASK_STOP_MAP_DI", strOptList, "{-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        //optList = split(strOptList, ':');
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(":"));
        funcIOMap.task_stop_di.type = std::stoi(optList[0]);
        funcIOMap.task_stop_di.index = std::stoi(optList[1]);
        // rcs_print("task_stop_di: %d %d\n", funcIOMap.task_stop_di.type,funcIOMap.task_stop_di.index);

        //const char* powerOnOptList = ini.Find("POWER_ON_MAP_DI", "FUNCTIONIO");
        ini.get("FUNCTIONIO.POWER_ON_MAP_DI", strOptList, "{-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        //optList = split(strOptList, ':');
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(":"));
        funcIOMap.power_on_di.type = std::stoi(optList[0]);
        funcIOMap.power_on_di.index = std::stoi(optList[1]);
        // rcs_print("power_on_di: %d %d\n", funcIOMap.power_on_di.type,funcIOMap.power_on_di.index);

        //const char* powerOffOptList = ini.Find("POWER_OFF_MAP_DI", "FUNCTIONIO");
        ini.get("FUNCTIONIO.POWER_OFF_MAP_DI", strOptList, "{-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        //optList = split(strOptList, ':');
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(":"));
        funcIOMap.power_off_di.type = std::stoi(optList[0]);
        funcIOMap.power_off_di.index = std::stoi(optList[1]);
        // rcs_print("power_off_di: %d %d\n", funcIOMap.power_off_di.type,funcIOMap.power_off_di.index);

        //const char* servoEnableOptList = ini.Find("SERVO_ENABLE_MAP_DI", "FUNCTIONIO");
        ini.get("FUNCTIONIO.SERVO_ENABLE_MAP_DI", strOptList, "{-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        //optList = split(strOptList, ':');
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(":"));
        funcIOMap.servo_enable_di.type = std::stoi(optList[0]);
        funcIOMap.servo_enable_di.index = std::stoi(optList[1]);
        // rcs_print("servo_enable_di: %d %d\n", funcIOMap.servo_enable_di.type,funcIOMap.servo_enable_di.index);

        //const char* servoDisableOptList = ini.Find("SERVO_DISABLE_MAP_DI", "FUNCTIONIO");
        ini.get("FUNCTIONIO.SERVO_DISABLE_MAP_DI", strOptList, "{-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        //optList = split(strOptList, ':');
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(":"));
        funcIOMap.servo_disable_di.type = std::stoi(optList[0]);
        funcIOMap.servo_disable_di.index = std::stoi(optList[1]);
        // rcs_print("servo_disable_di: %d %d\n", funcIOMap.servo_disable_di.type,funcIOMap.servo_disable_di.index);

        ////缩减整改 原缩减模式变为倍率模式，reduce_mode → percentage_mode,为保证兼容usersetting相关不做名称变更
        //const char* percentageModeLevelOneOptList = ini.Find("REDUCED_MODE_MAP_L1_DI", "FUNCTIONIO");
        ini.get("FUNCTIONIO.REDUCED_MODE_MAP_L1_DI", strOptList, "{-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        //optList = split(strOptList, ':');
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(":"));
        funcIOMap.percentage_mode_level1_di.type = std::stoi(optList[0]);
        funcIOMap.percentage_mode_level1_di.index = std::stoi(optList[1]);
        // rcs_print("percentage_mode_level1_di: %d %d\n", funcIOMap.percentage_mode_level1_di.type,funcIOMap.percentage_mode_level1_di.index);

        ////缩减整改 原缩减模式变为倍率模式，reduce_mode → percentage_mode,为保证兼容usersetting相关不做名称变更
        //const char* percentageModeLevelTwoOptList = ini.Find("REDUCED_MODE_MAP_L2_DI", "FUNCTIONIO");
        ini.get("FUNCTIONIO.REDUCED_MODE_MAP_L2_DI", strOptList, "{-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        //optList = split(strOptList, ':');
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(":"));
        funcIOMap.percentage_mode_level2_di.type = std::stoi(optList[0]);
        funcIOMap.percentage_mode_level2_di.index = std::stoi(optList[1]);
        // rcs_print("percentage_mode_level2_di: %d %d\n", funcIOMap.percentage_mode_level2_di.type,funcIOMap.percentage_mode_level2_di.index);

        //const char* stopModeOptList = ini.Find("STOP_MODE_MAP_DI", "FUNCTIONIO");
        ini.get("FUNCTIONIO.STOP_MODE_MAP_DI", strOptList, "{-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        //optList = split(strOptList, ':');
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(":"));
        funcIOMap.stop_mode_di.type = std::stoi(optList[0]);
        funcIOMap.stop_mode_di.index = std::stoi(optList[1]);
        // rcs_print("stop_mode_di: %d %d\n", funcIOMap.stop_mode_di.type,funcIOMap.stop_mode_di.index);

        //const char* backToIniposOptList = ini.Find("BACK_TO_INIPOS_MAP_DI", "FUNCTIONIO");
        ini.get("FUNCTIONIO.BACK_TO_INIPOS_MAP_DI", strOptList, "{-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        //optList = split(strOptList, ':');
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(":"));
        funcIOMap.back_to_inipos_di.type = std::stoi(optList[0]);
        funcIOMap.back_to_inipos_di.index = std::stoi(optList[1]);
        // rcs_print("backToIniposOptList: %d %d\n", funcIOMap.back_to_inipos_di.type,funcIOMap.back_to_inipos_di.index);

        //const char* clearErrorOptList = ini.Find("CLEAR_ERROR_DI", "FUNCTIONIO");
        ini.get("FUNCTIONIO.BACK_TO_INIPOS_MAP_DI", strOptList, "{-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        //optList = split(strOptList, ':');
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(":"));
        funcIOMap.clear_error_di.type = std::stoi(optList[0]);
        funcIOMap.clear_error_di.index = std::stoi(optList[1]);
        // rcs_print("clearErrorOptList: %d %d\n", funcIOMap.clear_error_di.type,funcIOMap.clear_error_di.index);

        //const char* dragModeOptList = ini.Find("DRAG_MODE_ENABLE_DI", "FUNCTIONIO");
        ini.get("FUNCTIONIO.DRAG_MODE_ENABLE_DI", strOptList, "{-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        //optList = split(strOptList, ':');
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(":"));
        funcIOMap.drag_mode_di.type = std::stoi(optList[0]);
        funcIOMap.drag_mode_di.index = std::stoi(optList[1]);
        rcs_print("dragModeOptList: %d %d\n", funcIOMap.drag_mode_di.type, funcIOMap.drag_mode_di.index);

        //const char* dragModeOffOptList = ini.Find("DRAG_MODE_DISABLE_DI", "FUNCTIONIO");
        ini.get("FUNCTIONIO.DRAG_MODE_DISABLE_DI", strOptList, "{-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        //optList = split(strOptList, ':');
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(":"));
        funcIOMap.drag_mode_disable_di.type = std::stoi(optList[0]);
        funcIOMap.drag_mode_disable_di.index = std::stoi(optList[1]);
        rcs_print("dragModeOffOptList: %d %d\n", funcIOMap.drag_mode_disable_di.type, funcIOMap.drag_mode_disable_di.index);

        //const char* idleOptList = ini.Find("TASK_IDLE_DO", "FUNCTIONIO");  //
        ini.get("FUNCTIONIO.TASK_IDLE_DO", strOptList, "{-1:-1,-1:-1,-1:-1,-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        //optList = split(strOptList, ',');
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(","));
        for (size_t i = 0; i < 4 && i < optList.size(); ++i)
        {
            // itemList = split(optList[i], ':');
            boost::algorithm::split(itemList, optList[i], boost::algorithm::is_any_of(":"));
            funcIOMap.task_idle_do[i].type = std::stoi(itemList[0]);
            funcIOMap.task_idle_do[i].index = std::stoi(itemList[1]);
            rcs_print("task_idle_do: %d %d\n", funcIOMap.task_idle_do[i].type, funcIOMap.task_idle_do[i].index);
        }

        //const char* pausedOptList = ini.Find("TASK_PAUSED_DO", "FUNCTIONIO");
        ini.get("FUNCTIONIO.TASK_PAUSED_DO", strOptList, "{-1:-1,-1:-1,-1:-1,-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        //optList = split(strOptList, ',');
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(","));
        for (size_t i = 0; i < 4 && i < optList.size(); ++i)
        {
            boost::algorithm::split(itemList, optList[i], boost::algorithm::is_any_of(":"));
            funcIOMap.task_paused_do[i].type = std::stoi(itemList[0]);
            funcIOMap.task_paused_do[i].index = std::stoi(itemList[1]);
            // rcs_print("task_paused_do: %d %d\n", funcIOMap.task_paused_do[i].type,funcIOMap.task_paused_do[i].index);
        }

        //const char* runningOptList = ini.Find("TASK_RUNNING_DO", "FUNCTIONIO");
        ini.get("FUNCTIONIO.TASK_RUNNING_DO", strOptList, "{-1:-1,-1:-1,-1:-1,-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(","));
        for (size_t i = 0; i < 4 && i < optList.size(); ++i)
        {
            boost::algorithm::split(itemList, optList[i], boost::algorithm::is_any_of(":"));
            funcIOMap.task_running_do[i].type = std::stoi(itemList[0]);
            funcIOMap.task_running_do[i].index = std::stoi(itemList[1]);
            // rcs_print("task_running_do: %d %d\n", funcIOMap.task_running_do[i].type,funcIOMap.task_running_do[i].index);
        }

        //const char* faultOptList = ini.Find("STAT_FAULT_DO", "FUNCTIONIO");
        ini.get("FUNCTIONIO.STAT_FAULT_DO", strOptList, "{-1:-1,-1:-1,-1:-1,-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(","));
        for (size_t i = 0; i < 4 && i < optList.size(); ++i)
        {
            boost::algorithm::split(itemList, optList[i], boost::algorithm::is_any_of(":"));
            funcIOMap.stat_fault_do[i].type = std::stoi(itemList[0]);
            funcIOMap.stat_fault_do[i].index = std::stoi(itemList[1]);
            // rcs_print("stat_fault_do: %d %d\n", funcIOMap.stat_fault_do[i].type,funcIOMap.stat_fault_do[i].index);
        }

        //const char* poweronOptList = ini.Find("STAT_POWERON_DO", "FUNCTIONIO");
        ini.get("FUNCTIONIO.STAT_POWERON_DO", strOptList, "{-1:-1,-1:-1,-1:-1,-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(","));
        for (size_t i = 0; i < 4 && i < optList.size(); ++i)
        {
            boost::algorithm::split(itemList, optList[i], boost::algorithm::is_any_of(":"));
            funcIOMap.stat_poweron_do[i].type = std::stoi(itemList[0]);
            funcIOMap.stat_poweron_do[i].index = std::stoi(itemList[1]);
            // rcs_print("stat_poweron_do: %d %d\n", funcIOMap.stat_poweron_do[i].type,funcIOMap.stat_poweron_do[i].index);
        }

        //const char* enabledOptList = ini.Find("STAT_ENABLED_DO", "FUNCTIONIO");
        ini.get("FUNCTIONIO.STAT_ENABLED_DO", strOptList, "{-1:-1,-1:-1,-1:-1,-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(","));
        for (size_t i = 0; i < 4 && i < optList.size(); ++i)
        {
            boost::algorithm::split(itemList, optList[i], boost::algorithm::is_any_of(":"));
            funcIOMap.stat_enabled_do[i].type = std::stoi(itemList[0]);
            funcIOMap.stat_enabled_do[i].index = std::stoi(itemList[1]);
            // rcs_print("stat_enabled_do: %d %d\n", funcIOMap.stat_enabled_do[i].type,funcIOMap.stat_enabled_do[i].index);
        }

        //const char* motionOptList = ini.Find("STAT_MOTION_DO", "FUNCTIONIO");
        ini.get("FUNCTIONIO.STAT_MOTION_DO", strOptList, "{-1:-1,-1:-1,-1:-1,-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(","));
        for (size_t i = 0; i < 4 && i < optList.size(); ++i)
        {
            boost::algorithm::split(itemList, optList[i], boost::algorithm::is_any_of(":"));
            funcIOMap.stat_motion_do[i].type = std::stoi(itemList[0]);
            funcIOMap.stat_motion_do[i].index = std::stoi(itemList[1]);
            // rcs_print("stat_motion_do: %d %d\n", funcIOMap.stat_motion_do[i].type,funcIOMap.stat_motion_do[i].index);
        }

        //const char* staticOptList = ini.Find("STAT_STATIC_DO", "FUNCTIONIO");
        ini.get("FUNCTIONIO.STAT_STATIC_DO", strOptList, "{-1:-1,-1:-1,-1:-1,-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(","));
        for (size_t i = 0; i < 4 && i < optList.size(); ++i)
        {
            boost::algorithm::split(itemList, optList[i], boost::algorithm::is_any_of(":"));
            funcIOMap.stat_static_do[i].type = std::stoi(itemList[0]);
            funcIOMap.stat_static_do[i].index = std::stoi(itemList[1]);
            // rcs_print("stat_static_do: %d %d\n", funcIOMap.stat_static_do[i].type,funcIOMap.stat_static_do[i].index);
        }

        //const char* bootOptList = ini.Find("STAT_BOOT_DO", "FUNCTIONIO");
        ini.get("FUNCTIONIO.STAT_BOOT_DO", strOptList, "{-1:-1,-1:-1,-1:-1,-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(","));
        for (size_t i = 0; i < 4 && i < optList.size(); ++i)
        {
            boost::algorithm::split(itemList, optList[i], boost::algorithm::is_any_of(":"));
            funcIOMap.stat_boot_do[i].type = std::stoi(itemList[0]);
            funcIOMap.stat_boot_do[i].index = std::stoi(itemList[1]);
            // rcs_print("stat_boot_do: %d %d\n", funcIOMap.stat_boot_do[i].type,funcIOMap.stat_boot_do[i].index);
        }

        //const char* emergencyStopOptList = ini.Find("STAT_EMERGEMCY_STOP_DO", "FUNCTIONIO");
        ini.get("FUNCTIONIO.STAT_EMERGEMCY_STOP_DO", strOptList, "{-1:-1,-1:-1,-1:-1,-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(","));
        for (size_t i = 0; i < 4 && i < optList.size(); ++i)
        {
            boost::algorithm::split(itemList, optList[i], boost::algorithm::is_any_of(":"));
            funcIOMap.stat_emergency_stop_do[i].type = std::stoi(itemList[0]);
            funcIOMap.stat_emergency_stop_do[i].index = std::stoi(itemList[1]);
            //rcs_print("stat_emergency_stop_do: %d %d\n", funcIOMap.stat_emergency_stop_do[i].type,funcIOMap.stat_emergency_stop_do[i].index);
        }

        //缩减整改 原缩减模式变为倍率模式，reduce_mode → percentage_mode,为保证兼容usersetting相关不做名称变更
        //const char* percentageModeLevelTwoStatOptList = ini.Find("STAT_REDUCED_MODE_DO", "FUNCTIONIO");
        ini.get("FUNCTIONIO.STAT_REDUCED_MODE_DO", strOptList, "{-1:-1,-1:-1,-1:-1,-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(","));
        for (size_t i = 0; i < 4 && i < optList.size(); ++i)
        {
            boost::algorithm::split(itemList, optList[i], boost::algorithm::is_any_of(":"));
            funcIOMap.stat_percentage_mode_level2_do[i].type = std::stoi(itemList[0]);
            funcIOMap.stat_percentage_mode_level2_do[i].index = std::stoi(itemList[1]);
            rcs_print(
                "stat_percentage_mode_level2_do: %d %d\n", funcIOMap.stat_percentage_mode_level2_do[i].type, funcIOMap.stat_percentage_mode_level2_do[i].index);
        }

        //const char* sysProtectiveStopOptList = ini.Find("STAT_SYS_PROTECTIVE_STOP_DO", "FUNCTIONIO");
        ini.get("FUNCTIONIO.STAT_SYS_PROTECTIVE_STOP_DO", strOptList, "{-1:-1,-1:-1,-1:-1,-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(","));
        for (size_t i = 0; i < 4 && i < optList.size(); ++i)
        {
            boost::algorithm::split(itemList, optList[i], boost::algorithm::is_any_of(":"));
            funcIOMap.stat_sys_protective_stop_do[i].type = std::stoi(itemList[0]);
            funcIOMap.stat_sys_protective_stop_do[i].index = std::stoi(itemList[1]);
            rcs_print("stat_sys_protective_stop_do: %d %d\n", funcIOMap.stat_sys_protective_stop_do[i].type, funcIOMap.stat_sys_protective_stop_do[i].index);
        }

        //const char* initialPoseOptList = ini.Find("STAT_INITIAL_POSE_DO", "FUNCTIONIO");
        ini.get("FUNCTIONIO.STAT_INITIAL_POSE_DO", strOptList, "{-1:-1,-1:-1,-1:-1,-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(","));
        for (size_t i = 0; i < 4 && i < optList.size(); ++i)
        {
            boost::algorithm::split(itemList, optList[i], boost::algorithm::is_any_of(":"));
            funcIOMap.stat_initial_pose_do[i].type = std::stoi(itemList[0]);
            funcIOMap.stat_initial_pose_do[i].index = std::stoi(itemList[1]);
            rcs_print("stat_initial_pose_do: %d %d\n", funcIOMap.stat_initial_pose_do[i].type, funcIOMap.stat_initial_pose_do[i].index);
        }

        //const char* percentageModeLevelOneStatOptList = ini.Find("STAT_PERCENTAGE_MODE_LEVEL1_DO", "FUNCTIONIO");
        ini.get("FUNCTIONIO.STAT_PERCENTAGE_MODE_LEVEL1_DO", strOptList, "{-1:-1,-1:-1,-1:-1,-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(","));
        for (int i = 0; i < 4 && i < (int)optList.size(); ++i)
        {
            boost::algorithm::split(itemList, optList[i], boost::algorithm::is_any_of(":"));
            funcIOMap.stat_percentage_mode_level1_do[i].type = std::stoi(itemList[0]);
            funcIOMap.stat_percentage_mode_level1_do[i].index = std::stoi(itemList[1]);
            rcs_print(
                "stat_percentage_mode_level1_do: %d %d\n", funcIOMap.stat_percentage_mode_level1_do[i].type, funcIOMap.stat_percentage_mode_level1_do[i].index);
        }

        //const char* dragModeStatOptList = ini.Find("STAT_DRAG_MODE_DO", "FUNCTIONIO");
        ini.get("FUNCTIONIO.STAT_DRAG_MODE_DO", strOptList, "{-1:-1,-1:-1,-1:-1,-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(","));
        for (int i = 0; i < 4 && i < (int)optList.size(); ++i)
        {
            boost::algorithm::split(itemList, optList[i], boost::algorithm::is_any_of(":"));
            funcIOMap.stat_drag_mode_do[i].type = std::stoi(itemList[0]);
            funcIOMap.stat_drag_mode_do[i].index = std::stoi(itemList[1]);
            rcs_print("stat_drag_mode_do: %d %d\n", funcIOMap.stat_drag_mode_do[i].type, funcIOMap.stat_drag_mode_do[i].index);
        }

        //const char* collisionModeStatOptList = ini.Find("STAT_COLLISION_MODE_DO", "FUNCTIONIO");
        ini.get("FUNCTIONIO.STAT_COLLISION_MODE_DO", strOptList, "{-1:-1,-1:-1,-1:-1,-1:-1}");
        strOptList.erase(0, strOptList.find_first_not_of("{"));
        strOptList.erase(strOptList.find_last_not_of("}") + 1);
        boost::algorithm::split(optList, strOptList, boost::algorithm::is_any_of(","));
        for (int i = 0; i < 4 && i < (int)optList.size(); ++i)
        {
            boost::algorithm::split(itemList, optList[i], boost::algorithm::is_any_of(":"));
            funcIOMap.stat_collision_mode_do[i].type = std::stoi(itemList[0]);
            funcIOMap.stat_collision_mode_do[i].index = std::stoi(itemList[1]);
            rcs_print("stat_collision_mode_do: %d %d\n", funcIOMap.stat_collision_mode_do[i].type, funcIOMap.stat_collision_mode_do[i].index);
        }
    }
    catch (...)
    {
    }

    /*set function IO map*/
    int res1 = zucSetFuncDIMap(funcIOMap, 0);
    int res2 = zucSetFuncDOMap(funcIOMap, 0);
    return res1 || res2;
}

static int zucInitSafetyFuncIOSettings(const char* inifile)
{
    std::vector<int> chnSetting;
    SafetyFuncDIMap sFuncDIMap;
    SafetyFuncDOMap sFuncDOMap;
    int drag_limit;

    if (inifile == nullptr)
    {
        return -1;
    }
    Util::IniParser ini_file(inifile);

    ini_file.get("SAFETYFUNCTIONIO.DI_CHN_FUNC_SETTINGS", chnSetting, {1, 1, 1, 1, 1, 1, 1, 1}, 1, 32);
    for (int i = 0; i < 8; i++) { sFuncDIMap.diChnFunc[i] = chnSetting[i]; }
    ini_file.get("SAFETYFUNCTIONIO.THREE_POSITION_ENABLING_DRAG_LIMIT", drag_limit, 0, 0, 1);
    sFuncDIMap.drag_limit = drag_limit;

    zucSetSafetyFuncDIMap(sFuncDIMap, 0);
    ini_file.get("SAFETYFUNCTIONIO.DO_CHN_FUNC_SETTINGS", chnSetting, {1, 1, 1, 1, 1, 1, 1, 1}, 1, 32);
    for (int i = 0; i < 8; i++) { sFuncDOMap.doChnFunc[i] = chnSetting[i]; }
    zucSetSafetyFuncDOMap(sFuncDOMap, 0);
    return 0;
}

static int zucInitSafetyRobotPowerLimitSettings(const char* inifile)
{
    Util::IniParser ini_file(inifile);

    // ZucIniFile trajInifile;
    // trajInifile.Open(inifile);
    try
    {
        double robotPowerLimit;
        ini_file.get("ROBOTSAFETYSETTING.ROBOT_POWER_LIMIT", robotPowerLimit, 1500.0, 0, __DBL_MAX__);
        // if (trajInifile.Find(&robotPowerLimit, "ROBOT_POWER_LIMIT", "ROBOTSAFETYSETTING") == ZucIniFile::ERR_NONE)
        // {
        int saveFlag = 0;
        if (robotPowerLimit > zucStatus->motion.robot_model_data.maxRobotPower)
        {
            saveFlag = 1;
            robotPowerLimit = zucStatus->motion.robot_model_data.maxRobotPower;
            rcs_print("robot_serial_num is '%d',  maxRobotPower = %f \n", zucStatus->motion.robot_serial_num, zucStatus->motion.robot_model_data.maxRobotPower);
        }
        if (robotPowerLimit < zucStatus->motion.robot_model_data.minRobotPower)
        {
            saveFlag = 1;
            robotPowerLimit = zucStatus->motion.robot_model_data.minRobotPower;
            rcs_print("robot_serial_num is '%d',  minRobotPower = %f \n", zucStatus->motion.robot_serial_num, zucStatus->motion.robot_model_data.minRobotPower);
        }
        zucSetRobotPowerLimit((float)robotPowerLimit, saveFlag);
    }
    catch (...)
    {
        rcs_print("robot power limit setup initialization failed\n");
        return -1;
    }

    return 0;
}

static int zucInitDragSpeedLimitSettings(const char* inifile)
{
    Util::IniParser ini_file(inifile);

    double dragTcpSpeedLimit;
    ini_file.get("ROBOTSAFETYSETTING.DRAG_TCP_SPEED_LIMIT", dragTcpSpeedLimit, 250.0, 50.0, 1500.0);

    zucStatus->motion.dragTcpSpeedLimit = dragTcpSpeedLimit;
    zucSetDragTcpSpeedLimit(dragTcpSpeedLimit);
    return 0;
}

static int zucInitRobotReduceSafetyLimitSettings(const char* inifile)
{
    // ZucIniFile trajInifile;
    // trajInifile.Open(inifile);
    Util::IniParser ini_file(inifile);
    try
    {
        double tcpVelLimitReduce, momentumLimitReduce, powerLimitReduce, stoppingDistReduce, stoppingTimeReduce, elbowVelLimitReduce;
        ini_file.get("ROBOTSAFETYSETTING.ELBOW_VEL_LIMIT_REDUCE", elbowVelLimitReduce, 250.0, 0.0, __DBL_MAX__);
        //if (trajInifile.Find(&elbowVelLimitReduce, "ELBOW_VEL_LIMIT_REDUCE", "ROBOTSAFETYSETTING") == ZucIniFile::ERR_NONE)
        zucStatus->reduceSafe.elbow_speed = elbowVelLimitReduce;
        zucSetRobotElbowVelLimitReduce(elbowVelLimitReduce);

        ini_file.get("ROBOTSAFETYSETTING.TCP_SPEED_LIMIT_REDUCE", tcpVelLimitReduce, 250.0, 0.0, __DBL_MAX__);
        // if (trajInifile.Find(&tcpVelLimitReduce, "TCP_SPEED_LIMIT_REDUCE", "ROBOTSAFETYSETTING") == ZucIniFile::ERR_NONE)
        zucStatus->reduceSafe.tcp_speed = tcpVelLimitReduce;
        zucSetRobotTcpVelLimitReduce(tcpVelLimitReduce);

        ini_file.get("ROBOTSAFETYSETTING.MOMENTUM_LIMIT_REDUCE", momentumLimitReduce, 2.5, 0.0, __DBL_MAX__);
        // if (trajInifile.Find(&momentumLimitReduce, "MOMENTUM_LIMIT_REDUCE", "ROBOTSAFETYSETTING") == ZucIniFile::ERR_NONE)
        rtapi_print("current MOMENTUM_LIMIT_REDUCE = %f \n", momentumLimitReduce);
        if (momentumLimitReduce > zucStatus->motion.robot_model_data.maxReduceMomentum)
        {
            momentumLimitReduce = zucStatus->motion.robot_model_data.maxReduceMomentum;
            rcs_print("robot_serial_num is '%d',  max momentumLimitReduce = %f \n",
                      zucStatus->motion.robot_serial_num,
                      zucStatus->motion.robot_model_data.maxReduceMomentum);
        }
        else if (momentumLimitReduce < zucStatus->motion.robot_model_data.minReduceMomentum)
        {
            momentumLimitReduce = zucStatus->motion.robot_model_data.minReduceMomentum;
            rcs_print("robot_serial_num is '%d',  min momentumLimitReduce = %f \n",
                      zucStatus->motion.robot_serial_num,
                      zucStatus->motion.robot_model_data.minReduceMomentum);
        }
        rtapi_print("check MOMENTUM_LIMIT_REDUCE = %f \n", momentumLimitReduce);
        zucStatus->reduceSafe.momentum_limit = momentumLimitReduce;
        zucSetRobotMomentumLimitReduce(momentumLimitReduce);
        reduce_param_config reduce_params;
        reduce_params.robot_momentum_limit_reduce = momentumLimitReduce;
        iniSaveReduceSafety(3, reduce_params);

        ini_file.get("ROBOTSAFETYSETTING.ROBOT_POWER_LIMIT_REDUCE", powerLimitReduce, 10.0, 0.0, __DBL_MAX__);
        // if (trajInifile.Find(&powerLimitReduce, "ROBOT_POWER_LIMIT_REDUCE", "ROBOTSAFETYSETTING") == ZucIniFile::ERR_NONE)
        rtapi_print("current ROBOT_POWER_LIMIT_REDUCE = %f \n", powerLimitReduce);
        if (powerLimitReduce > zucStatus->motion.robot_model_data.maxReduceRobotPower)
        {
            powerLimitReduce = zucStatus->motion.robot_model_data.maxReduceRobotPower;
            rcs_print("robot_serial_num is '%d',  max momentumLimitReduce = %f \n",
                      zucStatus->motion.robot_serial_num,
                      zucStatus->motion.robot_model_data.maxReduceRobotPower);
        }
        else if (powerLimitReduce < zucStatus->motion.robot_model_data.minReduceRobotPower)
        {
            powerLimitReduce = zucStatus->motion.robot_model_data.minReduceRobotPower;
            rcs_print("robot_serial_num is '%d',  min momentumLimitReduce = %f \n",
                      zucStatus->motion.robot_serial_num,
                      zucStatus->motion.robot_model_data.minReduceRobotPower);
        }
        rtapi_print("check ROBOT_POWER_LIMIT_REDUCE = %f \n", powerLimitReduce);
        zucStatus->reduceSafe.power_limit = powerLimitReduce;
        zucSetRobotPowerLimitReduce(powerLimitReduce);
        reduce_params.robot_poewr_limit_reduce = powerLimitReduce;
        iniSaveReduceSafety(4, reduce_params);
        ini_file.get("ROBOTSAFETYSETTING.STOPPING_DIST_REDUCE", stoppingDistReduce, 4000.0, 0.0, __DBL_MAX__);

        rtapi_print("check STOPPING_DIST_REDUCE = %f \n", stoppingDistReduce);
        zucStatus->reduceSafe.stopping_dist = stoppingDistReduce;
        zucSetRobotStoppingDistReduce(stoppingDistReduce);
        ini_file.get("ROBOTSAFETYSETTING.STOPPING_TIME_REDUCE", stoppingTimeReduce, 5.0, 0.0, __DBL_MAX__);
        rtapi_print("check STOPPING_TIME_REDUCE = %f \n", stoppingTimeReduce);
        zucStatus->reduceSafe.stopping_time = stoppingTimeReduce;
        zucSetRobotStoppingTimeReduce(stoppingTimeReduce);
    }
    catch (...)
    {
        rcs_print("robot safety limit setup initialization failed\n");
        return -1;
    }

    return 0;
}

static int zucInitRobotSafetyLimitSettings(const char* inifile)
{
    int model;
    // todo 请考虑重构整个快速设置碰撞的逻辑
    zucGetModelFromSerialNum(zucStatus->motion.robot_serial_num, &model);
    zucGetRobotSafety(&zucStatus->motion);
    rtapi_print("current custme type = %d model:%d\n", zucStatus->robotSafe[0].custome, model);
    iniSaveRobotSafety(model,
                       zucStatus->robotSafe[0].custome,
                       zucStatus->robotSafe[0].lastCustome,
                       0,
                       zucStatus->robotSafe[0].momentumLimit,
                       zucStatus->robotSafe[0].tcpVelLimit,
                       zucStatus->robotSafe[0].robotPowerLimit,
                       zucStatus->robotSafe[0].stoppingDistance,
                       zucStatus->robotSafe[0].stoppingTime);
    // ZucIniFile trajInifile;
    // trajInifile.Open(inifile);
    Util::IniParser ini_file(inifile);
    try
    {
        int saveFlag = 0;
        double momentumLimit, tcpVelLimit, stoppingDist, stoppingTime;
        ini_file.get("ROBOTSAFETYSETTING.MOMENTUM_LIMIT", momentumLimit, 70, 0.0, __DBL_MAX__);
        // if (trajInifile.Find(&momentumLimit, "MOMENTUM_LIMIT", "ROBOTSAFETYSETTING") == ZucIniFile::ERR_NONE)

        rtapi_print("momentumLimit = %f \n", momentumLimit);
        if (momentumLimit > zucStatus->motion.robot_model_data.maxMomentum)
        {
            saveFlag = 1;
            momentumLimit = zucStatus->motion.robot_model_data.maxMomentum;
            rtapi_print("robot_serial_num is '%d',  maxMomentum = %f \n", zucStatus->motion.robot_serial_num, zucStatus->motion.robot_model_data.maxMomentum);
        }
        if (momentumLimit < zucStatus->motion.robot_model_data.minMomentum)
        {
            saveFlag = 1;
            momentumLimit = zucStatus->motion.robot_model_data.minMomentum;
            rtapi_print("robot_serial_num is '%d',  minMomentum = %f \n", zucStatus->motion.robot_serial_num, zucStatus->motion.robot_model_data.minMomentum);
        }
        if (saveFlag)
        {
            iniSaveSafetyMomentumLimit(momentumLimit, zuc_inifile);
        }
        zucSetRobotMomentumLimit((float)momentumLimit);

        ini_file.get("ROBOTSAFETYSETTING.TCP_VEL_LIMIT", tcpVelLimit, 1350.0, 0.0, __DBL_MAX__);
        // if (trajInifile.Find(&tcpVelLimit, "TCP_VEL_LIMIT", "ROBOTSAFETYSETTING") == ZucIniFile::ERR_NONE)

        rtapi_print("tcpVelLimit = %f \n", tcpVelLimit);
        // 暂时不改
        // if(tcpVelLimit > zucStatus->motion.robot_model_data.maxTrajVel)
        // {
        //     saveFlag = 1;
        //     tcpVelLimit = zucStatus->motion.robot_model_data.maxTrajVel;
        //     rtapi_print("robot_serial_num is '%d',  maxTrajVel = %f \n",zucStatus->motion.robot_serial_num, zucStatus->motion.robot_model_data.maxTrajVel);
        // }

        // if(saveFlag) {
        //     iniSaveSafetyTCPVelLimit(tcpVelLimit, zuc_inifile);
        // }
        zucSetRobotTcpVelLimit((float)tcpVelLimit);

        ini_file.get("ROBOTSAFETYSETTING.STOPPING_DIST", stoppingDist, 4000.0, 0.0, __DBL_MAX__);
        //if (trajInifile.Find(&stoppingDist, "STOPPING_DIST", "ROBOTSAFETYSETTING") == ZucIniFile::ERR_NONE)

        rtapi_print("check STOPPING_DIST = %f \n", stoppingDist);
        zucSetRobotStoppingDist((float)stoppingDist);

        ini_file.get("ROBOTSAFETYSETTING.STOPPING_TIME", stoppingTime, 5.0, 0.0, __DBL_MAX__);
        rtapi_print("check STOPPING_TIME = %f \n", stoppingTime);
        zucSetRobotStoppingTime((float)stoppingTime);

        double robot_power_limit = 0.0;
        saveFlag = 0;

        ini_file.get("ROBOTSAFETYSETTING.ROBOT_POWER_LIMIT", robot_power_limit, 2000.0, 0.0, __DBL_MAX__);
        // if (trajInifile.Find(&robot_power_limit, "ROBOT_POWER_LIMIT", "ROBOTSAFETYSETTING") == ZucIniFile::ERR_NONE)
        // {
        rtapi_print("current ROBOT_POWER_LIMIT = %f \n", robot_power_limit);
        if (robot_power_limit > zucStatus->motion.robot_model_data.maxRobotPower)
        {
            saveFlag = 1;
            robot_power_limit = zucStatus->motion.robot_model_data.maxRobotPower;
            rtapi_print(
                "robot_serial_num is '%d',  robot_power_limit = %f \n", zucStatus->motion.robot_serial_num, zucStatus->motion.robot_model_data.maxRobotPower);
        }
        else if (robot_power_limit < zucStatus->motion.robot_model_data.minRobotPower)
        {
            saveFlag = 1;
            robot_power_limit = zucStatus->motion.robot_model_data.minRobotPower;
            rtapi_print(
                "robot_serial_num is '%d',  robot_power_limit = %f \n", zucStatus->motion.robot_serial_num, zucStatus->motion.robot_model_data.minRobotPower);
        }
        rtapi_print("check ROBOT_POWER_LIMIT = %f \n", robot_power_limit);
        zucSetRobotPowerLimit(robot_power_limit, saveFlag);
    }
    catch (...)
    {
        rcs_print("robot safety limit setup initialization failed\n");
        return -1;
    }

    return 0;
}

static int zucInitTIOSetup(const char* inifile)
{
    // ZucIniFile trajInifile;
    // trajInifile.Open(inifile);
    Util::IniParser ini_file(inifile);
    try
    {
        int robot_id = -1;

        int rs_chn_comm[5] = {0};
        std::vector<double> v_rs_chn_comm(5, 0);
        ini_file.get("TOOLIOSETTUP.RS485_CHN1_COMM", v_rs_chn_comm, {0, 115200, 8, 1, 78}, 0, INT_MAX);
        ModRtuComm rtuComm = {rs_chn_comm[0], rs_chn_comm[1], rs_chn_comm[2], rs_chn_comm[3], rs_chn_comm[4]};
        zucTioSetupRS485ChnComm(robot_id, 0, rtuComm);
        ini_file.get("TOOLIOSETTUP.RS485_CHN2_COMM", v_rs_chn_comm, {0, 115200, 8, 1, 78}, 0, INT_MAX);
        rtuComm = {rs_chn_comm[0], rs_chn_comm[1], rs_chn_comm[2], rs_chn_comm[3], rs_chn_comm[4]};
        zucTioSetupRS485ChnComm(robot_id, 1, rtuComm);
        int pin_cfg;
        ini_file.get("TOOLIOSETTUP.DI_PIN_CFG", pin_cfg, 0, INT_MIN, INT_MAX);
        zucTioSetPinMode(0, pin_cfg, 0);
        ini_file.get("TOOLIOSETTUP.DO_PIN_CFG", pin_cfg, 0, INT_MIN, INT_MAX);
        zucTioSetPinMode(1, pin_cfg, 0);
        ini_file.get("TOOLIOSETTUP.AI_PIN_CFG", pin_cfg, 0, INT_MIN, INT_MAX);
        zucTioSetPinMode(2, pin_cfg, 0);
        int rs_chn_mode;
        ini_file.get("TOOLIOSETTUP.RS485_CHN1_MODE", rs_chn_mode, 0, INT_MIN, INT_MAX);
        zucTioSetupRS485ChnMode(0, rs_chn_mode, 0);
        ini_file.get("TOOLIOSETTUP.RS485_CHN2_MODE", rs_chn_mode, 0, INT_MIN, INT_MAX);
        zucTioSetupRS485ChnMode(1, rs_chn_mode, 0);
        int voutEna = 0, voutVol = 0;
        ini_file.get("TOOLIOSETTUP.VOUT_ENABLE", (bool&)voutEna, false);
        zucTioSetVoutEnable(voutEna, 0);
        ini_file.get("TOOLIOSETTUP.VOUT_ENABLE", voutEna, 0);  //0:24v 1:12v tio协议定义如此
        zucTioSetVoutVoltage(voutVol, 0);
    }
    catch (...)
    {
        rcs_print("tio setup initialization failed\n");
        return -1;
    }

    // initialize the gripper signals
    int signalNum = 0;
    GripperStateUnit signals[MAX_TIO_SIG_NUM];
    xmlLoadTIOSignals(&signalNum, signals);
    rcs_print("signal num: %d\n", signalNum);
    for (int i = 0; i < signalNum; i++)
    {
        zucmotStatus.gripperState[i] = signals[i];
        gripperFreq[i] = signals[i].freq;
        zucTioMBAddSignal(0, signals[i].sigName, signals[i].chnId, signals[i].type, signals[i].addr);
    }

    return 0;
}

static int zucInitTorqSensorSetup(const char* filepath)
{
    int robot_id = -1;
    try
    {
        Payload payload;
        Util::IniParser ini_file(filepath);
        std::vector<double> v_centroid(3, 0);
        ini_file.get("TORQSENSOR.PAYLOADCENTROID", v_centroid, {0.0, 0.0, 20.833}, -2000.0, 2000.0);
        for (size_t i = 0; i < 3 && i < v_centroid.size(); i++) { payload.p[i] = v_centroid[i]; }
        ini_file.get("TORQSENSOR.PAYLOAD", payload.m, 0.449, 0.0, 2000.0);
        zucTorqSensorSetPayload(robot_id, payload);
        // rcs_print("torque sensor payload init: %f, %f, %f, %f\n", payload.m, payload.p[0], payload.p[1], payload.p[2]);

        /*torque sensor address*/
        int commType;
        CommInfo commCfg;
        ini_file.get("TORQSENSOR.COMM_TYPE", commType, 0);
        //commType = ini_getl("TORQSENSOR", "COMM_TYPE", 0, filepath);
        if (commType == 1)
        {
            // commCfg.rtuComm.baudrate = ini_getl("TORQSENSOR", "SENSOR_BAUDRATE", 115200, filepath);
            // commCfg.rtuComm.baudrate = ini_getl("TORQSENSOR", "SENSOR_DATABIT", 8, filepath);
            // commCfg.rtuComm.baudrate = ini_getl("TORQSENSOR", "SENSOR_STOPBIT", 1, filepath);
            // commCfg.rtuComm.baudrate = ini_getl("TORQSENSOR", "SENSOR_PARITY", 0, filepath);
            ini_file.get("TORQSENSOR.SENSOR_BAUDRATE", commCfg.rtuComm.baudrate, 0);
            ini_file.get("TORQSENSOR.SENSOR_DATABIT", commCfg.rtuComm.databits, 0);
            ini_file.get("TORQSENSOR.SENSOR_STOPBIT", commCfg.rtuComm.stopbits, 0);
            ini_file.get("TORQSENSOR.SENSOR_PARITY", commCfg.rtuComm.parity, 0);
        }
        else
        {
            std::string s_ipaddr;
            ini_file.get("TORQSENSOR.SENSOR_IP", s_ipaddr, "192.168.2.100");
            std::strncpy(commCfg.tcpComm.ipaddr, s_ipaddr.c_str(), sizeof(commCfg.tcpComm.ipaddr) - 1);
            commCfg.tcpComm.ipaddr[sizeof(commCfg.tcpComm.ipaddr) - 1] = '\0';
            //ini_gets("TORQSENSOR", "SENSOR_IP", "", commCfg.tcpComm.ipaddr, sizeof(commCfg.tcpComm.ipaddr) / sizeof(char), filepath);
            ini_file.get("TORQSENSOR.SENSOR_PORT", commCfg.tcpComm.port, 8080, 0, 65535);
            //commCfg.tcpComm.port = ini_getl("TORQSENSOR", "SENSOR_PORT", 0, filepath);
        }
        zucTorqSensorSetComm(0, commType, commCfg);

        /*restore the mode: enable at start*/
        int enable, brand;
        ini_file.get("TORQSENSOR.ENABLE", (bool&)enable, 0);
        //enable = ini_getl("TORQSENSOR", "ENABLE", 0, filepath);
        zucTorqSensorSetMode(enable, 0);
        ini_file.get("TORQSENSOR.SENSOR_BRAND", brand, 0);
        //brand = ini_getl("TORQSENSOR", "SENSOR_BRAND", 0, filepath);
        zucTorqSensorSetBrand(brand, 0);
        // enable = ini_getl("TORQSENSOR", "ENABLE", 0, filepath);
        // zucTorqSensorSetMode(enable, 0);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

static int zucInitNetExceptionHandle(const char* filepath)
{
    try
    {
        Util::IniParser ini_file(filepath);
        std::string tmp;
        ini_file.get("NETEXECEPTIONHANDLE.MOTTYPE", tmp, "MOT_KEEP");
        //ini_gets("NETEXECEPTIONHANDLE", "MOTTYPE", "MOT_KEEP", motType, sizeof(motType) / sizeof(char), filepath);
        if (tmp == "MOT_KEEP")
            zucStatus->netExceptionHandle.motType = MOT_KEEP;
        else if (tmp == "MOT_PAUSE")
        {
            zucStatus->netExceptionHandle.motType = MOT_PAUSE;
        }
        else if (tmp == "MOT_ABORT")
        {
            zucStatus->netExceptionHandle.motType = MOT_ABORT;
        }
        double timeLimit = 0.5;
        ini_file.get("NETEXECEPTIONHANDLE.TIMELIMIT", timeLimit, 0.5, 0.001, 60.0);
        zucStatus->netExceptionHandle.timeLimit = timeLimit;
        //zucStatus->netExceptionHandle.timeLimit = ini_getf("NETEXCEPTIONHANDLE", "TIMELIMIT", 0.0, filepath);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int zucSetFuncDIMap(FuncIOMap funcIOMap, int saveFlag)
{
    /*save the function io map into ini file*/
    if (saveFlag)
    {
        iniSaveFuncDIMap(funcIOMap, zuc_inifile);
    }
    zucmotCommand.command = ZUCMOT_SET_FUNCDI_MAP;
    // zucmotCommand.funcIOMap = funcIOMap;
    zucmotCommand.setfuncDIO.funcIOMap = funcIOMap;
    return usrmotWriteZucmotCommand(&zucmotCommand);
}

int zucSetFuncDOMap(FuncIOMap funcIOMap, int saveFlag)
{
    /*save the function io map into ini file*/
    if (saveFlag)
    {
        iniSaveFuncDOMap(funcIOMap, zuc_inifile);
    }
    zucmotCommand.command = ZUCMOT_SET_FUNCDO_MAP;
    // zucmotCommand.funcIOMap = funcIOMap;
    zucmotCommand.setfuncDIO.funcIOMap = funcIOMap;
    return usrmotWriteZucmotCommand(&zucmotCommand);
}

int zucSetSafetyFuncDIMap(SafetyFuncDIMap sFuncDIMap, int saveFlag)
{
    int ret;
    if (saveFlag)
    {
        iniSaveSafetyFuncDIMap(sFuncDIMap, zuc_inifile);
    }
    zucmotCommand.command = ZUCMOT_SET_SAFETY_FUNCDI_MAP;
    zucmotCommand.safetyFuncDIs.sFuncDISettings = sFuncDIMap;
    ret = usrmotWriteZucmotCommand(&zucmotCommand);
    if (task_init_finish_flag)
    {
        usrmotReadZucmotStatus(&zucmotStatus);
        iniChecksumPersistence(zucmotStatus.safetyCRC, CRC_FILE);
    }
    return ret;
}

int zucSetSafetyFuncDOMap(SafetyFuncDOMap sFuncDOMap, int saveFlag)
{
    int ret;
    if (saveFlag)
    {
        iniSaveSafetyFuncDOMap(sFuncDOMap, zuc_inifile);
    }
    zucmotCommand.command = ZUCMOT_SET_SAFETY_FUNCDO_MAP;
    zucmotCommand.safetyFuncDOs.sFuncDOSettings = sFuncDOMap;
    ret = usrmotWriteZucmotCommand(&zucmotCommand);
    if (task_init_finish_flag)
    {
        usrmotReadZucmotStatus(&zucmotStatus);
        iniChecksumPersistence(zucmotStatus.safetyCRC, CRC_FILE);
    }
    return ret;
}

int zucSetRobotPowerLimit(float robotPowerLimit, int saveFlag)
{
    int ret;
    if (saveFlag)
    {
        iniSaveSafetyRobotPowerLimit(robotPowerLimit, zuc_inifile);
    }
    auto cmd = std::make_shared<CmdSetRobotPowerLimit>(0, robotPowerLimit);
    return mot::realtime_cmd_call(cmd);
    if (task_init_finish_flag)
    {
        usrmotReadZucmotStatus(&zucmotStatus);
        iniChecksumPersistence(zucmotStatus.safetyCRC, CRC_FILE);
    }
    return ret;
}

int zucSetCabinetId(unsigned int cab_id, int saveFlag)
{
    if (saveFlag)
    {
        iniSaveCabinetId(cab_id, zuc_inifile);
    }
    auto cmd = std::make_shared<CmdSetCabinetId>(cab_id);
    return mot::realtime_cmd_call(cmd);
}

double zucJointGetMaxVelocity(int joint)
{
    if (joint < 0 || joint >= ZUCMOT_MAX_JOINTS)
    {
        return 0;
    }
    return JointConfig[joint].MaxVel;
}

double zucJointGetMaxAcceleration(int joint)
{
    if (joint < 0 || joint >= ZUCMOT_MAX_JOINTS)
    {
        return 0;
    }
    return JointConfig[joint].MaxAccel;
}

int zucMonitorUpdate(ZUC_MONITOR_STAT* stat)
{
    static int lastCabinetID = zucmotStatus.cab_status.cabinetID;
    auto scb = rtdev::RtDevMgr::instance().get_scb();
    strncpy(stat->scb_ver_str, scb->cab()->get_version_str().c_str(), sizeof(stat->scb_ver_str) - 1);
    stat->scb_hw_version = scb->cab()->get_hw_ver();
    stat->scb_sw_version = scb->cab()->get_sw_ver();
    stat->cabTemperature = scb->cab()->get_temperature();
    stat->robotAveragePower = scb->rob_power()->get_power(0);
    stat->robotAverageCurrent = scb->rob_power()->get_current(0);
    stat->robotVoltage = scb->rob_power()->get_voltage(0);
    stat->cabinetID = scb->cab()->get_id();
    stat->currRunTime = scb->cab()->get_current_run_time();
    stat->accuRunTime = scb->cab()->get_total_run_time();
    stat->commState = zucmotStatus.commState;
    for (int i = 0; i < zucStatus->motion.robot_joint_num; i++) { stat->jointMonitorData[i] = zucmotStatus.servo_status.jointMonitorData[i]; }

    if (lastCabinetID != (int)zucmotStatus.cab_status.cabinetID)
    {
        rcs_print("set the cabid, last = %d, new = %d\n", lastCabinetID, zucmotStatus.cab_status.cabinetID);
        zucSetCabinetId(zucmotStatus.cab_status.cabinetID, 1);
    }
    lastCabinetID = zucmotStatus.cab_status.cabinetID;
    return 0;
}

//added by alex.li at 2018.08.02 for using traj.maxaccel
double zucTrajGetMaxAcceleration(void) { return TrajConfig.MaxAccel; }

//added by alex.li at 2018.08.02 for using traj.maxvel
double zucTrajGetMaxVel(void)
{
    // rtapi_print(" ************************** tcpVelLimit = %f, TrajConfig.MaxVel = %f, tcpVelLimit = %f\n", tcpVelLimit,
    //     TrajConfig.MaxVel, tcpVelLimit);
    // if(tcpVelLimit < TrajConfig.MaxVel){
    //     return tcpVelLimit;
    // }
    return TrajConfig.MaxVel;
}

double zucAxisGetMaxAcceleration(int axis)
{
    if (axis < 0 || axis >= ZUCMOT_MAX_AXIS)
    {
        return 0;
    }

    return AxisConfig[axis].MaxAccel;
}

int zucJogMovc(int mvOpt,
               double* pos_mid,
               double* pos_end,
               double vel,
               double acc,
               double tol,
               int circle_cnt,
               int executing_line_id,
               int* end_cond,
               int circlemode)
{
    if (pos_mid == nullptr || pos_end == nullptr)
        return -1;

    if (zucmotStatus.protective_stop == 1)
    {
        rcs_print("Cannot do movc when in protective_stop status\n");
        return -1;
    }

    // if not in auto mode, swith to auto mode
    if (zucStatus->task.mode != ZUC_TASK_MODE_AUTO)
    {
        zucTrajSetMode(ZUC_TRAJ_MODE_COORD);
    }
    // @FIXME 这些参数从zucmotcommand中删除了，会影响toppa
    zuc_jog_t jog = {};
    jog.cartPose.tran.x = pos_end[0];
    jog.cartPose.tran.y = pos_end[1];
    jog.cartPose.tran.z = pos_end[2];
    jog.cartPose.a = pos_end[3];
    jog.cartPose.b = pos_end[4];
    jog.cartPose.c = pos_end[5];
    jog.cartPose.u = 0;
    jog.cartPose.v = 0;
    jog.cartPose.w = 0;
    jog.cartPoseEx.tran.x = pos_mid[0];
    jog.cartPoseEx.tran.y = pos_mid[1];
    jog.cartPoseEx.tran.z = pos_mid[2];
    jog.cartPoseEx.a = pos_mid[3];
    jog.cartPoseEx.b = pos_mid[4];
    jog.cartPoseEx.c = pos_mid[5];
    jog.cartPoseEx.u = 0;
    jog.cartPoseEx.v = 0;
    jog.cartPoseEx.w = 0;
    jog.vel = vel < TrajConfig.MaxVel ? vel : TrajConfig.MaxVel;
    jog.acc = acc;
    jog.acc = acc < TrajConfig.MaxAccel ? acc : TrajConfig.MaxAccel;
    jog.tol = tol;
    jog.executing_line_id = executing_line_id;
    for (int i = 0; i < 3; i++) { jog.end_cond[i] = end_cond[i]; }
    jog.moveOpt = ((mvOpt >> 4) & 0xF);  // 1 for jog mode
    jog.circle_cnt = circle_cnt;
    jog.circle_mode = circlemode;

    int type = 0;
    if (mvOpt & 0xF)
        type = 1;
    else
        type = 0;

    auto cmd = std::make_shared<CmdJogMovc>(0, type, jog);
    return mot::realtime_cmd_call(cmd);
}

int zucJointLoadComp(int joint, const char* file, int type)
{  // 意义不明
    return usrmotLoadComp(joint, file, type);
}

/*
  these globals are set in zucMotionUpdate(), then referenced in
  zucJointUpdate(), zucTrajUpdate() to save calls to usrmotReadZucmotStatus
 */
static zucmot_debug_t zucmotDebug;
static int errcode;
static char errorString[ZUCMOT_ERROR_LEN];
static int new_config = 0;

// #define WATCH_FLAGS 1

// ZUC_TRAJ functions

int zucTrajSetMode(int mode)
{
    if (zucmotStatus.protective_stop == 1 && mode == ZUC_TRAJ_MODE_DRAG)
    {
        auto cmd = std::make_shared<CmdExitProtect>(0, 0);
        return mot::realtime_cmd_call(cmd);
    }

    if (zucmotStatus.protective_stop == 1 && mode != ZUC_TRAJ_MODE_FREE)
    {
        rcs_print("Cannot change to non-free mode when in protective_stop status\n");
        return -1;
    }

    switch (mode)
    {
    case ZUC_TRAJ_MODE_FREE: {
        for (int i = 0;; i++)
        {
            auto cmd = std::make_shared<CmdFree>(i);
            int err = mot::realtime_cmd_call(cmd);
            if (err)
            {
                return err;
            }
        }
    }

    case ZUC_TRAJ_MODE_COORD: {
        auto cmd = std::make_shared<CmdCoord>(0);
        return mot::realtime_cmd_call(cmd);
    }

    case ZUC_TRAJ_MODE_TELEOP: {
        auto cmd = std::make_shared<CmdTeleop>(0);
        return mot::realtime_cmd_call(cmd);
    }

    case ZUC_TRAJ_MODE_TELEOP_TOOL: {
        auto cmd = std::make_shared<CmdTeleopTool>(0);
        return mot::realtime_cmd_call(cmd);
    }

    case ZUC_TRAJ_MODE_SERVOJOP: {
        // auto cmd = std::make_shared<CmdServojop>(0,
        //                                          TrajConfig.nlf_jp,
        //                                          TrajConfig.mmf_buf_size,
        //                                          TrajConfig.nlf_vr,
        //                                          TrajConfig.nlf_jr,
        //                                          TrajConfig.mmf_ka,
        //                                          TrajConfig.mmf_kv,
        //                                          TrajConfig.nlf_ar,
        //                                          TrajConfig.mmf_kp,
        //                                          TrajConfig.filterType,
        //                                          TrajConfig.nlf_vp,
        //                                          TrajConfig.nlf_ap,
        //                                          TrajConfig.lpf_cutoff_freq);
        // return mot::realtime_cmd_call(cmd);
    }

    case ZUC_TRAJ_MODE_ADMITTANCE: {
        auto cmd = std::make_shared<CmdAdmittance>(0);
        return mot::realtime_cmd_call(cmd);
    }

    case ZUC_TRAJ_MODE_DRAG: {
        auto cmd = std::make_shared<CmdDrag>(0);
        return mot::realtime_cmd_call(cmd);
    }

    default:
        return -1;
    }
    return 0;
}

int zucTrajSetScale(double scale)
{
    // 意义不明
    if (scale < 0.0)
    {
        scale = 0.0;
    }

    auto cmd = std::make_shared<CmdFeedScale>(0, scale);
    return mot::realtime_cmd_call(cmd);
}

int zucTrajSetFOEnable(unsigned char mode)
{
    auto cmd = std::make_shared<CmdFsEnable>(0, mode);
    return mot::realtime_cmd_call(cmd);
}

int zucTrajSetSOEnable(unsigned char mode)
{
    auto cmd = std::make_shared<CmdSsEnable>(0, mode);
    return mot::realtime_cmd_call(cmd);
}

int zucTrajSetAFEnable(unsigned char enable)
{
    auto cmd = std::make_shared<CmdAfEnable>(0, enable);
    return mot::realtime_cmd_call(cmd);
}

int zucTrajSetMotionId(int id)
{
    // rtapi_print("enter [zucTrajSetMotionId], id = %d\n", id);
    if (ZUC_DEBUG_MOTION_TIME & zuc_debug)
    {
        if (id != TrajConfig.MotionId)
        {
            rcs_print("Outgoing motion id is %d.\n", id);
        }
    }

    TrajConfig.MotionId = id;

    return 0;
}

int zucTrajGetUseTopp(int use_topp)
{
    userset_use_toppra = use_topp;
    if (unable_use_toppra)
    {
        userset_use_toppra = 0;
        rtapi_print(" pyimport failed --- unable_use_toppra \n");
    }
    if (/* use_topp &&  */ blend_seg_jpos.jpos == NULL)
    {
        rtapi_print("[zucTrajGetUseTopp]  ---> start malloc blend_seg_jpos space\n");
        blend_seg_jpos.jpos = (BlendJpos*)malloc(sizeof(BlendJpos) * MAX_BLEND_LEN);
        if (blend_seg_jpos.jpos == NULL)
        {
            rtapi_print("[zucTrajGetUseTopp]  ---> failed malloc blend_seg_jpos space\n");
        }
        else
        {
            blend_seg_jpos.rear = 0;
            blend_seg_jpos.front = 0;
            rtapi_print("[zucTrajGetUseTopp]  ---> succeed malloc blend_seg_jpos space\n");
        }
    }
    rtapi_print(" ************************************************** use_topp = %d ************************************************** \n", userset_use_toppra);
    return 0;
}

int zucTrajGetFilterSize(int filter_size)
{
    userset_filter_size = filter_size;
    rtapi_print(" ************************************************** userset_filter_size = %d ************************************************** \n",
                userset_filter_size);
    return 0;
}

int zucTrajGetAddTorCons(int add_tor_cons)
{
    useradd_tor_cons = add_tor_cons;
    rtapi_print(" ************************************************** useradd_tor_cons = %d ************************************************** \n",
                useradd_tor_cons);
    return 0;
}

int zucTrajGetTorPortion(double tor_portion)
{
    usertor_portion = tor_portion;
    //为避免影响到动力学相关算法，设置比例进行关节力矩限制范围的缩小
    if (usertor_portion > 1.0)
    {
        usertor_portion = 1.0;
    }
    else if (usertor_portion < 0.5)
    {
        usertor_portion = 0.5;
    }
    return 0;
}

int zucTrajSetDisJLen(double dislen)
{
    zucmotCommand.disjlen = dislen;
    rtapi_print(" ************************************************** zucmotCommand.disjlen = %f ************************************************** \n",
                zucmotCommand.disjlen);
    return 0;
}

int zucTrajSetDisLLen(double dislen)
{
    zucmotCommand.disllen = dislen;
    rtapi_print(" ************************************************** zucmotCommand.disllen = %f ************************************************** \n",
                zucmotCommand.disllen);
    return 0;
}

int zucTrajSetDisCLen(double dislen)
{
    zucmotCommand.disclen = dislen;
    rtapi_print(" ************************************************** zucmotCommand.disclen = %f ************************************************** \n",
                zucmotCommand.disclen);
    return 0;
}

int zucTrajSetSmoothPara(double smooth_para)
{
    zucmotCommand.smooth_para = smooth_para;
    rtapi_print(" ************************************************** zucmotCommand.smooth_para = %f ************************************************** \n",
                zucmotCommand.smooth_para);
    return 0;
}

int zucTrajSetRateBlendPara(int rate_blend, int topp_queue_len, int max_fw_len)
{
    auto cmd = std::make_shared<CmdSetRateBlend>(0, rate_blend, topp_queue_len, max_fw_len);
    return mot::realtime_cmd_call(cmd);
}

double zucTrajGetMaxJointJerk(void) { return zucmotConfig.joint_jerk; }

double zucTrajGetMaxCarteJerk(void) { return zucmotConfig.carte_jerk; }

//added by alex.li at 2018.08.03

int zucTrajDisableForceControl(int disableForceCtrl)
{
    // @FIXME liwang 意义不明
    zucStatus->motion.disableForceCtrl = disableForceCtrl;
    iniSaveCompliantTypeEnable(0, zucStatus->motion.compliantEnable, zuc_inifile);
    auto cmd = std::make_shared<CmdDisableForceControl>(0, disableForceCtrl);
    return mot::realtime_cmd_call(cmd);
}

int zucTrajSetAdmittanceEnable(int enable)
{
    auto cmd = std::make_shared<CmdSetAdmittanceEna>(0, enable);
    return mot::realtime_cmd_call(cmd);
}

int zucTrajSetAdmittanceOption(int* option)
{
    auto cmd = std::make_shared<CmdSetAdmittanceOpt>(0, option);
    return mot::realtime_cmd_call(cmd);
}

int zucTrajSetEndForceCond(int axis, EndForceCond endForceCond)
{
    auto cmd = std::make_shared<CmdSetEndForceCondition>(0, axis, endForceCond);
    return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucTrajAbort()
{
    auto cmd = std::make_shared<CmdAbort>(0);
    return mot::realtime_cmd_call(cmd);
}

int zucTrajStep()
{
    auto cmd = std::make_shared<CmdStep>(0);
    return mot::realtime_cmd_call(cmd);
}

int zucTrajDelay(double delay)
{
    /* nothing need be done here - it's done in task controller */

    return 0;
}

int zucTrackingConfig(int link_di, int com_cycle_num, double synch_acc, int lctsynch)
{
    auto cmd = std::make_shared<CmdSetTrackingConfig>(0, link_di, lctsynch, synch_acc, com_cycle_num);
    return mot::realtime_cmd_call(cmd);
}

int zucSetDHCom(int full_dh_com)
{
    // setFullDHCom(full_dh_com);
    zucStatus->motion.full_dh_com = full_dh_com;

    auto cmd = std::make_shared<CmdSetFullDhCom>(0, full_dh_com);
    return mot::realtime_cmd_call(cmd);
}

int zucSetLimitLinearVelocity(int enable)
{
    auto cmd = std::make_shared<CmdSetEnableLimitLinearVelocity>(0, enable);
    return mot::realtime_cmd_call(cmd);
}

int zucSetLimitJointVelocity(int enable)
{
    auto cmd = std::make_shared<CmdSetEnableLimitJointVelocity>(0, enable);
    return mot::realtime_cmd_call(cmd);
}

int zucSetAttitudeSafety(int choice, double tilt, double pan, double deviation, double deviation_warning, double cenrpy[3], int enable_open, int enable_auto)
{
    // 安全相关
    int ret;

    zuc_set_attitude_safety setattitudesafety = {};
    setattitudesafety.conical_tip_choice = choice;
    setattitudesafety.tool_properties_tilt = tilt;
    setattitudesafety.tool_properties_pan = pan;
    setattitudesafety.deviation = deviation;
    setattitudesafety.deviation_warning = deviation_warning;
    setattitudesafety.enableLimAttitudeSafetyOpen = enable_open;
    setattitudesafety.enableLimAttitudeSafetyAuto = enable_auto;
    for (int i = 0; i < 3; i++) { setattitudesafety.conical_cenrpy[i] = cenrpy[i]; }

    if (task_init_finish_flag)
    {
        usrmotReadZucmotStatus(&zucmotStatus);
        iniChecksumPersistence(zucmotStatus.safetyCRC, CRC_FILE);
    }

    auto cmd = std::make_shared<CmdSetAttitudeSafety>(0, setattitudesafety);
    return mot::realtime_cmd_call(cmd);
}

int zucSetPlannerJerk(double joint_jerk, double carte_jerk)
{
    auto cmd = std::make_shared<CmdSetPlannerJerk>(0, joint_jerk, carte_jerk);
    return mot::realtime_cmd_call(cmd);
}

int zucSetTcpMaxError(double max_error)
{
    if (max_error < 1.0)
    {
        max_error = 1.0;
        rtapi_print("tcpmaxerror cannot set to be lower than 1.0\n");
    }
    auto cmd = std::make_shared<CmdSetTcpMaxError>(0, max_error);
    return mot::realtime_cmd_call(cmd);
}

int zucSetPBblendMode(int pb_blend_mode)
{
    auto cmd = std::make_shared<CmdSetPbblendmode>(0, pb_blend_mode);
    return mot::realtime_cmd_call(cmd);
}

//Send blend flag (int enable) to the controller
int zucSetMovejMovelBlend(int enable)
{
    auto cmd = std::make_shared<CmdSetEnableMovejmovelblend>(0, enable);
    return mot::realtime_cmd_call(cmd);
}

//Send blend flag (int enable) to the controller
int zucSetMovejMovejBlend(int enable)
{
    auto cmd = std::make_shared<CmdSetEnableMovejmovejblend>(0, enable);
    return mot::realtime_cmd_call(cmd);
}

int zucSetCartJogAcc(double* step_jog_pos_acc, double* step_jog_ori_acc)
{
    auto cmd = std::make_shared<CmdSetCartJogAcc>(0, step_jog_ori_acc, step_jog_pos_acc);
    return mot::realtime_cmd_call(cmd);
}

int zucTrajSetSafeAttitude(PmRpy conicalRpy,
                           double tilt,
                           double pan,
                           double deviation,
                           double deviationwarn,
                           int openEnable,
                           int autoEnable,
                           int tipChoice,
                           int saveOption)
{
    //printf("3tipChoice = %d\n",tipChoice);
    if ((-180 <= conicalRpy.y && conicalRpy.y <= 180) && (-180 <= conicalRpy.p && conicalRpy.p <= 180) && (-180 <= conicalRpy.r && conicalRpy.r <= 180) &&
        (-360 <= tilt && tilt <= 360) && (-360 <= pan && pan <= 360) && (5 <= deviation && deviation <= 180) &&
        (0 <= deviationwarn && deviationwarn <= deviation))
    {
        printf("zucTrajSetSafeAttitude: paramaters check  [ PASS ] .\n");
    }
    else
    {
        printf("zucTrajSetSafeAttitude: paramaters check  [ FAILED ] .\n");
        return 0;
    }
    if (saveOption)
    {
        iniSaveSafeAttitude(conicalRpy, tilt, pan, deviation, deviationwarn, openEnable, autoEnable, tipChoice, zuc_inifile);
    }
    double cenrpy[3];
    cenrpy[0] = conicalRpy.y;
    cenrpy[1] = conicalRpy.p;
    cenrpy[2] = conicalRpy.r;
    //printf("222tipChoice = %d\n",tipChoice);
    zucSetAttitudeSafety(tipChoice, tilt, pan, deviation, deviationwarn, cenrpy, openEnable, autoEnable);
    return 0;
}

int zucTrajConveyorLinearEnable(double x, double y, double z, double pulseEquivalent, int convyr_type, double max_distance)
{
    //  rtapi_print("----------zucTrajConveyorLinearEnable----------:  x = %f y=%f z=%f unit=%f   \n",x,y,z,pulseEquivalent);
    zucmotCommand.command = ZUCMOT_TRACKING_ENABLE;
    zucmotCommand.enaconvyrtrack.convyr_type = convyr_type;
    zucmotCommand.enaconvyrtrack.conveyorDir.x = x;
    zucmotCommand.enaconvyrtrack.conveyorDir.y = y;
    zucmotCommand.enaconvyrtrack.conveyorDir.z = z;
    zucmotCommand.enaconvyrtrack.conveyorPulseEquivalent = pulseEquivalent;
    zucmotCommand.enaconvyrtrack.max_distance = max_distance;
    if (fabs(x) < 1e-8 && fabs(y) < 1e-8 && fabs(z) < 1e-8)
    {
        zucOperatorError(ERROR_NO_TRACKING_DIR, "linear conveyor tracking direction is zero");
        return -1;
    }
    return usrmotWriteZucmotCommand(&zucmotCommand);
}

PmCartesian get_conveyor_center(PmCartesian A, PmCartesian B, PmCartesian C)
{
    PmCartesian u, v;    //求出的新坐标系X,Y方向上的向量
    PmCartesian center;  //圆心
    //PmCartesian AB, AC;  // AB: 向量AB， AC：向量AC
    PmCartesian ex, ey;  //ex: 新坐标系X轴单位向量 ，ey: 新坐标系Y轴单位向量
    PmCartesian vy, vx;  // C点做垂先和AB相交，所得的X轴上的向量。
    double xB, xC, yC;   // AB的长度。

    PmCartesian tempEx, tempEy;
    //计算 u = B - A;
    pmCartCartSub(&B, &A, &u);  //计算 v = C - A;
    pmCartCartSub(&C, &A, &v);  //计算 xB = norm(u)
    pmCartMag(&u, &xB);         //计算 ex = u/xB
    pmCartUnit(&u, &ex);        //vx = （v * ex）*ex;
    pmCartCartDot(&v, &ex, &xC);
    pmCartScalMult(&ex, xC, &vx);  //计算 vy = v - vx;
    pmCartCartSub(&v, &vx, &vy);   //计算Y轴单位向量 ey = ey/|ey|
    pmCartUnit(&vy, &ey);          //计算 yC = |vy|
    pmCartMag(&vy, &yC);           //计算center = A + 1/2 xB*ex + (-xB (xB - xC) xC + xB yC^2)/(2 xB yC)*ey
    pmCartScalMult(&ex, 0.5 * xB, &tempEx);
    pmCartCartAdd(&A, &tempEx, &tempEx);
    pmCartScalMult(&ey, (-1 * xB * (xB - xC) * xC + xB * yC * yC) / (2 * xB * yC), &tempEy);
    pmCartCartAdd(&tempEx, &tempEy, &center);

    return center;
}

int zucTrajConveyorCircularEnable(double p1x,
                                  double p1y,
                                  double p1z,
                                  double p2x,
                                  double p2y,
                                  double p2z,
                                  double p3x,
                                  double p3y,
                                  double p3z,
                                  double pulseEquivalent,
                                  int convyr_type,
                                  int rotate_tool,
                                  double max_distance)
{
    // rtapi_print("enter zucTrajConveyorCircularEnable\n");
    //根据给定三点坐标(当前用户坐标系下获得)计算传送带圆心坐标系在当前用户坐标系下的变换矩阵
    PmCartesian pos1, pos2, pos3, z0, x0, y0;
    //double theta;
    pos1.x = p1x;
    pos1.y = p1y;
    pos1.z = p1z;
    pos2.x = p2x;
    pos2.y = p2y;
    pos2.z = p2z;
    pos3.x = p3x;
    pos3.y = p3y;
    pos3.z = p3z;

    double sm[3] = {0.0};
    double me[3] = {0.0};
    double se[3] = {0.0};
    double ms_norm = 0.0, me_norm = 0.0, se_norm = 0.0;
    sm[0] = pos1.x - pos2.x;
    sm[1] = pos1.y - pos2.y;
    sm[2] = pos1.z - pos2.z;
    me[0] = pos3.x - pos2.x;
    me[1] = pos3.y - pos2.y;
    me[2] = pos3.z - pos2.z;
    se[0] = -pos1.x + pos3.x;
    se[1] = -pos1.y + pos3.y;
    se[2] = -pos1.z + pos3.z;
    ms_norm = sqrt(sm[0] * sm[0] + sm[1] * sm[1] + sm[2] * sm[2]);
    me_norm = sqrt(me[0] * me[0] + me[1] * me[1] + me[2] * me[2]);
    se_norm = sqrt(se[0] * se[0] + se[1] * se[1] + se[2] * se[2]);

    double area = 0.5 * (ms_norm + me_norm + se_norm);
    if (area * (area - ms_norm) * (area - me_norm) * (area - se_norm) < 1e-5)
    {
        zucOperatorError(ERROR_P1P2P3_COLINEAR, "ERROR: circular conveyor P1, P2 and P3 are colinear");
        return -1;
    }
    /* 标定方法1： 如果标定的是传送带上共圆的三点, p1, p2, p3确定传送带圆心坐标o(p1, p2, p3一定是在世界坐标系下的坐标, 圆心o也是世界坐标系下的坐标), (op1)坐标系+x, (op1)叉乘(op2)是轴线方向即+z,(+z)叉乘(op1)是+y*/
    PmCartesian center;
    center = get_conveyor_center(pos1, pos2, pos3);
    pmCartCartSub(&pos1, &center, &x0);
    pmCartCartSub(&pos3, &center, &y0);
    pmCartUnitEq(&x0);
    pmCartUnitEq(&y0);
    pmCartCartCross(&x0, &y0, &z0);
    pmCartUnitEq(&z0);
    pmCartCartCross(&z0, &x0, &y0);
    zucmotCommand.enaconvyrtrack.o_to_world.tran.x = center.x;
    zucmotCommand.enaconvyrtrack.o_to_world.tran.y = center.y;
    zucmotCommand.enaconvyrtrack.o_to_world.tran.z = center.z;
    // rtapi_print("position of o_conveyor are: %f %f %f\n", center.x, center.y, center.z);

    /* 标定方法2： 如果标定的是传送带轴线方向, p1是圆心, p1p2方向是轴线方向, (p1p2)叉乘(p2p3)是坐标系+y, (+y)叉乘(p1p2)是坐标系+x */
    // pmCartCartSub(&pos2, &pos1, &z0);
    // pmCartUnitEq(&z0);
    // pmCartCartSub(&pos3, &pos2, &x0);
    // pmCartUnitEq(&x0);
    // pmCartCartDot(&z0,&x0,&theta);

    // if((1-theta)<0.001)
    // {
    //     rtapi_print("Circle conveyor failed ---> inappropriate points! \n");
    //     return -1;
    // }
    // pmCartCartCross(&z0, &x0,&y0);
    // pmCartUnitEq(&y0);
    // pmCartCartCross(&y0, &z0,&x0);
    // pmCartUnitEq(&x0);
    // rtapi_print("[zucTrajConveyorCircularEnable] ---> x0 = %f %f %f, y0 = %f %f %f, z0 = %f %f %f\n", x0.x, x0.y, x0.z, y0.x, y0.y, y0.z, z0.x, z0.y, z0.z);
    // zucmotCommand.enaconvyrtrack.o_to_world.tran.x = p1x;
    // zucmotCommand.enaconvyrtrack.o_to_world.tran.y = p1y;
    // zucmotCommand.enaconvyrtrack.o_to_world.tran.z = p1z;

    zucmotCommand.enaconvyrtrack.o_to_world.rot.x = x0;
    zucmotCommand.enaconvyrtrack.o_to_world.rot.y = y0;
    zucmotCommand.enaconvyrtrack.o_to_world.rot.z = z0;

    zucmotCommand.enaconvyrtrack.convyr_type = convyr_type;
    zucmotCommand.enaconvyrtrack.conveyorPulseEquivalent = pulseEquivalent;
    zucmotCommand.enaconvyrtrack.rotate_tool = rotate_tool;
    zucmotCommand.enaconvyrtrack.max_distance = max_distance;
    zucmotCommand.command = ZUCMOT_TRACKING_ENABLE;
    return usrmotWriteZucmotCommand(&zucmotCommand);
}

int zucTrajConveyorDisable(void)
{
    // rtapi_print("----------zucTrajConveyorDisable----------:    \n");
    auto cmd = std::make_shared<CmdTrackingDisable>(0);
    return mot::realtime_cmd_call(cmd);
}

int zucToppraFinishLeftTc(void)
{
    // rtapi_print("----------zucToppraFinishLeftTc----------:    \n");
    first_command = 1;
    kinematic_error = 0;

    auto cmd = std::make_shared<CmdToppraFinishLeftTc>(0);
    return mot::realtime_cmd_call(cmd);
}

int zucToppraFirstCommand(void)
{
    // rtapi_print(" ---------- zucToppraFirstCommand ---------- :    \n");
    first_command = 1;
    kinematic_error = 0;
    lasttc_left_len = 0;
    if (blend_seg_jpos.jpos != NULL)
    {
        blend_seg_jpos.rear = 0;
        blend_seg_jpos.front = 0;
    }
    else
    {
        rtapi_print(" ************ blend_seg_jpos == NULL ************ \n");
    }

    auto cmd = std::make_shared<CmdToppraFirstCommand>(0);
    return mot::realtime_cmd_call(cmd);
}

int zucSetFunctionSaftyPara(const double* robot_power_limit,
                            const double* robot_momentum_limit,
                            const double* robot_endforce_limit,
                            double robot_velocity_limit)
{
    // for (int i = 0; i < 2; i++) {
    // 	zucStatus->motion.robot_power_limit[i] = robot_power_limit[i];
    // 	zucStatus->motion.robot_momentum_limit[i] = robot_momentum_limit[i];
    // 	zucStatus->motion.robot_endforce_limit[i] = robot_endforce_limit[i];
    // }
    // zucStatus->motion.robot_velocity_limit = robot_velocity_limit

    rtapi_print("zucmotCommand.functionsaftypara.robot_power_limit = %lf\n", zucmotCommand.functionsaftypara.robot_power_limit[1]);
    rtapi_print("zucmotCommand.functionsaftypara.robot_momentum_limit = %lf\n", zucmotCommand.functionsaftypara.robot_momentum_limit[1]);
    rtapi_print("zucmotCommand.functionsaftypara.robot_endforce_limit = %lf\n", zucmotCommand.functionsaftypara.robot_endforce_limit[1]);
    auto cmd = std::make_shared<CmdSetFunctionSaftyPara>(0, robot_momentum_limit, robot_endforce_limit, robot_velocity_limit, robot_power_limit);
    return mot::realtime_cmd_call(cmd);
}

static int checkCalcTcpResult(const ZucPose tcpPose, const ZucPose* flangPose, size_t flangPoseCnt)
{
    //tcp的长度必须在一定范围内
    double tcpLen = sqrt(tcpPose.tran.x * tcpPose.tran.x + tcpPose.tran.y * tcpPose.tran.y + tcpPose.tran.z * tcpPose.tran.z);
    if (tcpLen > 2000.0)
    {
        rtapi_print("tcp calibration position is over workspace limit\n");
        return -1;
    }
    //tcp的误差要在一定范围内
    //TODO:
    return 0;
}

int calculate_center_circle(ZucPose const* const point1, ZucPose const* const point2, ZucPose const* const point3, PmCartesian* center_cir)
{
    if (!point1 || !point2 || !point3)
        return -1;
    double collinear =
        fabs((point3->tran.y - point1->tran.y) * (point2->tran.x - point1->tran.x) - (point2->tran.y - point1->tran.y) * (point3->tran.x - point1->tran.x));
    if (collinear < TP_POS_EPSILON)
    {
        rtapi_print("collinear is %f\n", collinear);

        return -1;
    }
    double x1 = point1->tran.x;
    double y1 = point1->tran.y;
    double z1 = point1->tran.z;
    double x2 = point2->tran.x;
    double y2 = point2->tran.y;
    double z2 = point2->tran.z;
    double x3 = point3->tran.x;
    double y3 = point3->tran.y;
    double z3 = point3->tran.z;

    double a1 = y1 * z2 - y2 * z1 - y1 * z3 + y3 * z1 + y2 * z3 - y3 * z2;
    double b1 = -(x1 * z2 - x2 * z1 - x1 * z3 + x3 * z1 + x2 * z3 - x3 * z2);
    double c1 = x1 * y2 - x2 * y1 - x1 * y3 + x3 * y1 + x2 * y3 - x3 * y2;
    double d1 = -(x1 * y2 * z3 - x1 * y3 * z2 - x2 * y1 * z3 + x2 * y3 * z1 + x3 * y1 * z2 - x3 * y2 * z1);
    double a2 = 2 * (x2 - x1);
    double b2 = 2 * (y2 - y1);
    double c2 = 2 * (z2 - z1);
    double d2 = pmSq(x1) + pmSq(y1) + pmSq(z1) - pmSq(x2) - pmSq(y2) - pmSq(z2);
    double a3 = 2 * (x3 - x1);
    double b3 = 2 * (y3 - y1);
    double c3 = 2 * (z3 - z1);
    double d3 = pmSq(x1) + pmSq(y1) + pmSq(z1) - pmSq(x3) - pmSq(y3) - pmSq(z3);
    center_cir->x = -(b1 * c2 * d3 - b1 * c3 * d2 - b2 * c1 * d3 + b2 * c3 * d1 + b3 * c1 * d2 - b3 * c2 * d1) /
                    (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1);
    center_cir->y = (a1 * c2 * d3 - a1 * c3 * d2 - a2 * c1 * d3 + a2 * c3 * d1 + a3 * c1 * d2 - a3 * c2 * d1) /
                    (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1);
    center_cir->z = -(a1 * b2 * d3 - a1 * b3 * d2 - a2 * b1 * d3 + a2 * b3 * d1 + a3 * b1 * d2 - a3 * b2 * d1) /
                    (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1);
    //r=pmSqrt(pmSq(x1-center_cir->x)+pmSq(y1-center_cir->y)+pmSq(z1-center_cir->z));
    return 0;
}
int calcTCPErr_dist(ZucPose const* const tool_to_user1,
                    ZucPose const* const tool_to_user2,
                    ZucPose const* const tool_to_user3,
                    ZucPose const* const tool_to_user4,
                    double* const dist_mean,
                    int* const num)
{
    if (!tool_to_user1 || !tool_to_user2 || !tool_to_user3 || !tool_to_user4)
        return -1;
    ZucPose tool_mean;

    tool_mean.tran.x = (tool_to_user1->tran.x + tool_to_user2->tran.x + tool_to_user3->tran.x + tool_to_user4->tran.x) / 4;
    tool_mean.tran.y = (tool_to_user1->tran.y + tool_to_user2->tran.y + tool_to_user3->tran.y + tool_to_user4->tran.y) / 4;
    tool_mean.tran.z = (tool_to_user1->tran.z + tool_to_user2->tran.z + tool_to_user3->tran.z + tool_to_user4->tran.z) / 4;

    dist_mean[0] = pmSqrt(pmSq(tool_mean.tran.x - tool_to_user1->tran.x) + pmSq(tool_mean.tran.y - tool_to_user1->tran.y) +
                          pmSq(tool_mean.tran.z - tool_to_user1->tran.z));
    dist_mean[1] = pmSqrt(pmSq(tool_mean.tran.x - tool_to_user2->tran.x) + pmSq(tool_mean.tran.y - tool_to_user2->tran.y) +
                          pmSq(tool_mean.tran.z - tool_to_user2->tran.z));
    dist_mean[2] = pmSqrt(pmSq(tool_mean.tran.x - tool_to_user3->tran.x) + pmSq(tool_mean.tran.y - tool_to_user3->tran.y) +
                          pmSq(tool_mean.tran.z - tool_to_user3->tran.z));
    dist_mean[3] = pmSqrt(pmSq(tool_mean.tran.x - tool_to_user4->tran.x) + pmSq(tool_mean.tran.y - tool_to_user4->tran.y) +
                          pmSq(tool_mean.tran.z - tool_to_user4->tran.z));
    *num = 4;

    return 0;
}

int calcTCPErr_lea_squ(const MatrixXd& A, const VectorXd& B, const VectorXd& retTcp, double* const dist_mean, int* const num)
{
    // if(!A||!B||!retTcp)
    //     return -1;
    PmCartesian error_lea_squ[6] = {0.0};
    for (int i = 0; i < 6; i++)
    {
        error_lea_squ[i].x = (A(i * 3, 0) * retTcp(0) + A(i * 3, 1) * retTcp(1) + A(i * 3, 2) * retTcp(2)) - B(i * 3);
        error_lea_squ[i].y = (A(i * 3 + 1, 0) * retTcp(0) + A(i * 3 + 1, 1) * retTcp(1) + A(i * 3 + 1, 2) * retTcp(2)) - B(i * 3 + 1);
        error_lea_squ[i].z = (A(i * 3 + 2, 0) * retTcp(0) + A(i * 3 + 2, 1) * retTcp(1) + A(i * 3 + 2, 2) * retTcp(2)) - B(i * 3 + 2);
        dist_mean[i] = pmSqrt(pmSq(error_lea_squ[i].x) + pmSq(error_lea_squ[i].y) + pmSq(error_lea_squ[i].z));
    }
    *num = 6;
    return 0;
}

int calcTCPErr_center_cir(ZucPose const* const tool_to_user1,
                          ZucPose const* const tool_to_user2,
                          ZucPose const* const tool_to_user3,
                          ZucPose const* const tool_to_user4,
                          double* const dist_mean,
                          int* const num)
{
    if (!tool_to_user1 || !tool_to_user2 || !tool_to_user3 || !tool_to_user4)
        return -1;
    PmCartesian cneter_cir1, cneter_cir2, cneter_cir3, cneter_cir4, center_mean;
    double r1, r2, r3, r4;
    calculate_center_circle(tool_to_user1, tool_to_user2, tool_to_user3, &cneter_cir1);
    calculate_center_circle(tool_to_user1, tool_to_user2, tool_to_user4, &cneter_cir2);
    calculate_center_circle(tool_to_user1, tool_to_user3, tool_to_user4, &cneter_cir3);
    calculate_center_circle(tool_to_user2, tool_to_user3, tool_to_user4, &cneter_cir4);
    center_mean.x = (cneter_cir1.x + cneter_cir2.x + cneter_cir3.x + cneter_cir4.x) / 4;
    center_mean.y = (cneter_cir1.y + cneter_cir2.y + cneter_cir3.y + cneter_cir4.y) / 4;
    center_mean.z = (cneter_cir1.z + cneter_cir2.z + cneter_cir3.z + cneter_cir4.z) / 4;
    dist_mean[0] =
        pmSqrt(pmSq(center_mean.x - tool_to_user1->tran.x) + pmSq(center_mean.y - tool_to_user1->tran.y) + pmSq(center_mean.z - tool_to_user1->tran.z));
    dist_mean[1] =
        pmSqrt(pmSq(center_mean.x - tool_to_user2->tran.x) + pmSq(center_mean.y - tool_to_user2->tran.y) + pmSq(center_mean.z - tool_to_user2->tran.z));
    dist_mean[2] =
        pmSqrt(pmSq(center_mean.x - tool_to_user3->tran.x) + pmSq(center_mean.y - tool_to_user3->tran.y) + pmSq(center_mean.z - tool_to_user3->tran.z));
    dist_mean[3] =
        pmSqrt(pmSq(center_mean.x - tool_to_user4->tran.x) + pmSq(center_mean.y - tool_to_user4->tran.y) + pmSq(center_mean.z - tool_to_user4->tran.z));
    *num = 4;
    return 0;
}

int zucTrajCalcTcp(const unsigned char id,
                   const unsigned char saveType,
                   const unsigned char statusFlag,
                   const double* jntPos1,
                   const double* jntPos2,
                   const double* jntPos3,
                   const double* jntPos4)
{
    double permissible_min_error = 100;
    unsigned long curr_iflags = 0, curr_fflags = 0;
    ZucPose cartPose[4] = {0.0};
    ZucPose flangeTool;
    ZERO_ZUC_POSE(flangeTool);
    if (fabs(jntPos1[0]) + fabs(jntPos1[1]) + fabs(jntPos1[2]) + fabs(jntPos1[3]) + fabs(jntPos1[4]) + fabs(jntPos1[5]) > 0.01)
        axisgroup::AxisGroupMgr::instance().get_axisgroup(0)->kine()->fkine(RobJointVal(jntPos1), cartPose[0]);
    if (fabs(jntPos2[0]) + fabs(jntPos2[1]) + fabs(jntPos2[2]) + fabs(jntPos2[3]) + fabs(jntPos2[4]) + fabs(jntPos2[5]) > 0.01)
        axisgroup::AxisGroupMgr::instance().get_axisgroup(0)->kine()->fkine(RobJointVal(jntPos2), cartPose[1]);
    if (fabs(jntPos3[0]) + fabs(jntPos3[1]) + fabs(jntPos3[2]) + fabs(jntPos3[3]) + fabs(jntPos3[4]) + fabs(jntPos3[5]) > 0.01)
        axisgroup::AxisGroupMgr::instance().get_axisgroup(0)->kine()->fkine(RobJointVal(jntPos3), cartPose[2]);
    if (fabs(jntPos4[0]) + fabs(jntPos4[1]) + fabs(jntPos4[2]) + fabs(jntPos4[3]) + fabs(jntPos4[4]) + fabs(jntPos4[5]) > 0.01)
        axisgroup::AxisGroupMgr::instance().get_axisgroup(0)->kine()->fkine(RobJointVal(jntPos4), cartPose[3]);

    usrOpeErrCode = 0;
    ZERO_ZUC_POSE(utilCartPos);
    tcpCalError[0] = 0.0;
    tcpCalError[1] = 0.0;
    tcpCalError[2] = 0.0;
    if (saveType == 1)
    {
        //判断四点位姿是否重合
        double dist_p[6] = {0.0};
        int num_dist = 0;
        for (int i = 0; i < 4; i++)
        {
            for (int j = i + 1; j < 4; j++)
            {
                dist_p[num_dist] = pmSqrt(pmSq(cartPose[i].tran.x - cartPose[j].tran.x) + pmSq(cartPose[i].tran.y - cartPose[j].tran.y) +
                                          pmSq(cartPose[i].tran.z - cartPose[j].tran.z));

                if (dist_p[num_dist] < TP_POS_EPSILON)
                {
                    zucOperatorError(REPEATED_TEACH_POINTS, "teach points are repeated");
                    return -1;
                }
                num_dist++;
            }
        }
        PmRpy temprpy;
        PmRotationMatrix rotMat, rotMatList[4];
        VectorXd B(6 * 3), retTcp(3);
        MatrixXd A(6 * 3, 3);
        VectorXd deltaP(6 * 3);
        int mtrixIndex = 0;
        for (int i = 0; i < 4; i++)
        {
            temprpy.y = cartPose[i].a * PM_PI / 180.0;
            temprpy.p = cartPose[i].b * PM_PI / 180.0;
            temprpy.r = cartPose[i].c * PM_PI / 180.0;
            pmRpyMatConvert(&temprpy, &rotMat);
            for (int j = i - 1; j >= 0; j--)
            {
                A(3 * mtrixIndex, 0) = rotMat.x.x - rotMatList[j].x.x;
                A(3 * mtrixIndex, 1) = rotMat.y.x - rotMatList[j].y.x;
                A(3 * mtrixIndex, 2) = rotMat.z.x - rotMatList[j].z.x;
                A(3 * mtrixIndex + 1, 0) = rotMat.x.y - rotMatList[j].x.y;
                A(3 * mtrixIndex + 1, 1) = rotMat.y.y - rotMatList[j].y.y;
                A(3 * mtrixIndex + 1, 2) = rotMat.z.y - rotMatList[j].z.y;
                A(3 * mtrixIndex + 2, 0) = rotMat.x.z - rotMatList[j].x.z;
                A(3 * mtrixIndex + 2, 1) = rotMat.y.z - rotMatList[j].y.z;
                A(3 * mtrixIndex + 2, 2) = rotMat.z.z - rotMatList[j].z.z;
                B(3 * mtrixIndex) = cartPose[j].tran.x - cartPose[i].tran.x;
                B(3 * mtrixIndex + 1) = cartPose[j].tran.y - cartPose[i].tran.y;
                B(3 * mtrixIndex + 2) = cartPose[j].tran.z - cartPose[i].tran.z;
                mtrixIndex++;
            }
            rotMatList[i] = rotMat;
        }

        retTcp = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(B);
        utilCartPos.tran.x = retTcp(0);  //计算得到的工具坐标系
        utilCartPos.tran.y = retTcp(1);
        utilCartPos.tran.z = retTcp(2);
        if (checkCalcTcpResult(utilCartPos, cartPose, 4))
        {
            zucOperatorError(TCP_CALIBRATION_POSITION_OVER_LIMIT, "tcp calibration position is over workspace limit");
            ZERO_ZUC_POSE(utilCartPos);
            return -1;
        }

        ZucPose tool_to_user1, tool_to_user2, tool_to_user3, tool_to_user4;
        axisgroup::AxisGroupMgr::instance().get_axisgroup(0)->kine()->fkine(
            RobJointVal(jntPos1), tool_to_user1, &utilCartPos, &flangeTool, &zucmotConfig.base_offset);
        axisgroup::AxisGroupMgr::instance().get_axisgroup(0)->kine()->fkine(
            RobJointVal(jntPos2), tool_to_user2, &utilCartPos, &flangeTool, &zucmotConfig.base_offset);
        axisgroup::AxisGroupMgr::instance().get_axisgroup(0)->kine()->fkine(
            RobJointVal(jntPos3), tool_to_user3, &utilCartPos, &flangeTool, &zucmotConfig.base_offset);
        axisgroup::AxisGroupMgr::instance().get_axisgroup(0)->kine()->fkine(
            RobJointVal(jntPos4), tool_to_user4, &utilCartPos, &flangeTool, &zucmotConfig.base_offset);

        double dist_mean[6] = {0.0};
        int num = 0;
        switch (TCP_ERROR_TYPE)
        {
        case 0: {
            calcTCPErr_dist(&tool_to_user1, &tool_to_user2, &tool_to_user3, &tool_to_user4, dist_mean, &num);
            break;
        }
        case 1: {
            calcTCPErr_lea_squ(A, B, retTcp, dist_mean, &num);
            break;
        }
        case 2: {
            calcTCPErr_center_cir(&tool_to_user1, &tool_to_user2, &tool_to_user3, &tool_to_user4, dist_mean, &num);
            break;
        }
        default: {
            rtapi_print("tcp error type choice failed\n");
            return -1;
        }
        }

        double max_error = dist_mean[0], min_error = dist_mean[0], mean_error = dist_mean[0];

        for (int i = 1; i < num; i++)
        {
            if (max_error < dist_mean[i])
            {
                max_error = dist_mean[i];
            }
            if (min_error > dist_mean[i])
            {
                min_error = dist_mean[i];
            }
            mean_error += dist_mean[i];
        }
        mean_error = mean_error / num;
        tcpCalError[0] = max_error;
        tcpCalError[1] = min_error;
        tcpCalError[2] = mean_error;
        rtapi_print("max error is %f, min error is %f, mean error is %f\n", max_error, min_error, mean_error);
        if (permissible_min_error < min_error)
        {
            zucOperatorError(TCP_CALIBRATION_ERROR_TOO_BIG, "tcp calibration error is too big");
            rtapi_print("min error is %f less then permissible min error is %f\n", min_error, permissible_min_error);
            tcpCalError[0] = 0.0;
            tcpCalError[1] = 0.0;
            tcpCalError[2] = 0.0;
            return -1;
        }
    }
    iniSaveTCPCalibration(0, id, saveType, statusFlag, cartPose, jntPos1, jntPos2, jntPos3, jntPos4, 0, 0, tcpCalError);
    return 0;
}

int zucTrajCalcTcp_six(const unsigned char id,
                       const unsigned char saveType,
                       const unsigned char statusFlag,
                       const double* jntPos1,
                       const double* jntPos2,
                       const double* jntPos3,
                       const double* jntPos4,
                       const double* jntPos5,
                       const double* jntPos6)
{
    unsigned long curr_iflags = 0, curr_fflags = 0;
    ZucPose cartPose[6] = {0.0};
    ZucPose flangeTool;
    ZERO_ZUC_POSE(flangeTool);
    usrOpeErrCode = 0;

    // calculate TCP position using 4-point method
    if (zucTrajCalcTcp(id, saveType, statusFlag, jntPos1, jntPos2, jntPos3, jntPos4) != 0)
    {
        return -1;
    }

    /* calculate TCP gesture */
    auto kine = axisgroup::AxisGroupMgr::instance().get_axisgroup(0)->kine();
    if (fabs(jntPos4[0]) + fabs(jntPos4[1]) + fabs(jntPos4[2]) + fabs(jntPos4[3]) + fabs(jntPos4[4]) + fabs(jntPos4[5]) > 0.01)
        kine->fkine(RobJointVal(jntPos4), cartPose[3]);
    if (fabs(jntPos5[0]) + fabs(jntPos5[1]) + fabs(jntPos5[2]) + fabs(jntPos5[3]) + fabs(jntPos5[4]) + fabs(jntPos5[5]) > 0.01)
        kine->fkine(RobJointVal(jntPos5), cartPose[4]);
    if (fabs(jntPos6[0]) + fabs(jntPos6[1]) + fabs(jntPos6[2]) + fabs(jntPos6[3]) + fabs(jntPos6[4]) + fabs(jntPos6[5]) > 0.01)
        kine->fkine(RobJointVal(jntPos6), cartPose[5]);

    if (saveType)
    {
        //判断四五六点是否共线(共点可以接受)
        PmCartesian vec34, vec45, vec35;
        vec34.x = cartPose[4].tran.x - cartPose[3].tran.x;
        vec34.y = cartPose[4].tran.y - cartPose[3].tran.y;
        vec34.z = cartPose[4].tran.z - cartPose[3].tran.z;

        vec45.x = cartPose[5].tran.x - cartPose[4].tran.x;
        vec45.y = cartPose[5].tran.y - cartPose[4].tran.y;
        vec45.z = cartPose[5].tran.z - cartPose[4].tran.z;

        vec35.x = cartPose[5].tran.x - cartPose[3].tran.x;
        vec35.y = cartPose[5].tran.y - cartPose[3].tran.y;
        vec35.z = cartPose[5].tran.z - cartPose[3].tran.z;

        double vec34_mags = vec34.x * vec34.x + vec34.y * vec34.y + vec34.z * vec34.z;
        double vec45_mags = vec45.x * vec45.x + vec45.y * vec45.y + vec45.z * vec45.z;
        double vec35_mags = vec35.x * vec35.x + vec35.y * vec35.y + vec35.z * vec35.z;
        int three_p_repeat = (vec34_mags > 1e-8 && vec45_mags > 1e-8 && vec35_mags > 1e-8) ? 0 : 1;
        if (three_p_repeat)
        {
            zucOperatorError(REPEATED_TEACH_POINTS, "teach points are repeated");
            return -1;
        }
        pmCartUnit(&vec34, &vec34);
        pmCartUnit(&vec45, &vec45);
        PmCartesian corss_ret;
        pmCartCartCross(&vec34, &vec45, &corss_ret);
        double corss_ret_mag = pmSqrt(corss_ret.x * corss_ret.x + corss_ret.y * corss_ret.y + corss_ret.z * corss_ret.z);
        if (corss_ret_mag < 1e-4 && !three_p_repeat)
        {
            zucOperatorError(TEACH_POINTS_COLLINEAR, "teach points are collinear");
            return -1;
        }

        PmRpy temprpy;
        PmRotationMatrix rotMat, utilrotTcp, rotbf;
        Matrix3f rotfb(3, 3), rotTcptoB(3, 3), retrotTcp(3, 3);
        temprpy.y = cartPose[3].a * PM_PI / 180.0;
        temprpy.p = cartPose[3].b * PM_PI / 180.0;
        temprpy.r = cartPose[3].c * PM_PI / 180.0;
        pmRpyMatConvert(&temprpy, &rotMat);
        rotfb << rotMat.x.x, rotMat.y.x, rotMat.z.x, rotMat.x.y, rotMat.y.y, rotMat.z.y, rotMat.x.z, rotMat.y.z, rotMat.z.z;
        rotbf.x.x = rotMat.x.x;
        rotbf.x.y = rotMat.y.x;
        rotbf.x.z = rotMat.z.x;
        rotbf.y.x = rotMat.x.y;
        rotbf.y.y = rotMat.y.y;
        rotbf.y.z = rotMat.z.y;
        rotbf.z.x = rotMat.x.z;
        rotbf.z.y = rotMat.y.z;
        rotbf.z.z = rotMat.z.z;
        pmMatRpyConvert(&rotbf, &temprpy);

        Vector3f rotTcptoB_z(cartPose[4].tran.x - cartPose[3].tran.x, cartPose[4].tran.y - cartPose[3].tran.y, cartPose[4].tran.z - cartPose[3].tran.z);
        Vector3f rotTcptoB_x(cartPose[5].tran.x - cartPose[4].tran.x, cartPose[5].tran.y - cartPose[4].tran.y, cartPose[5].tran.z - cartPose[4].tran.z);
        Vector3f rotTcptoB_y;
        rotTcptoB_y = rotTcptoB_z.cross(rotTcptoB_x);
        rotTcptoB_x = rotTcptoB_y.cross(rotTcptoB_z);
        rotTcptoB_x.normalize();
        rotTcptoB_y.normalize();
        rotTcptoB_z.normalize();
        rotTcptoB << rotTcptoB_x(0), rotTcptoB_y(0), rotTcptoB_z(0), rotTcptoB_x(1), rotTcptoB_y(1), rotTcptoB_z(1), rotTcptoB_x(2), rotTcptoB_y(2),
            rotTcptoB_z(2);
        rtapi_print("the y,p,r of world to flange are: %f, %f, %f\n", temprpy.y * 180 / PM_PI, temprpy.p * 180 / PM_PI, temprpy.r * 180 / PM_PI);

        retrotTcp = rotfb.inverse() * rotTcptoB;
        utilrotTcp.x.x = retrotTcp(0, 0);
        utilrotTcp.x.y = retrotTcp(1, 0);
        utilrotTcp.x.z = retrotTcp(2, 0);
        utilrotTcp.y.x = retrotTcp(0, 1);
        utilrotTcp.y.y = retrotTcp(1, 1);
        utilrotTcp.y.z = retrotTcp(2, 1);
        utilrotTcp.z.x = retrotTcp(0, 2);
        utilrotTcp.z.y = retrotTcp(1, 2);
        utilrotTcp.z.z = retrotTcp(2, 2);
        pmMatRpyConvert(&utilrotTcp, &temprpy);

        utilCartPos.a = temprpy.y * 180 / PM_PI;
        utilCartPos.b = temprpy.p * 180 / PM_PI;
        utilCartPos.c = temprpy.r * 180 / PM_PI;
    }
    iniSaveTCPCalibration(1, id, saveType, statusFlag, cartPose, jntPos1, jntPos2, jntPos3, jntPos4, jntPos5, jntPos6, 0);
    return 0;
}

int zucTrajCalcSafeAtt(const ZucPose* cartpos1, const ZucPose* cartpos2, const ZucPose* cartpos3, const double* tilt, const double* pan, const int* tipChoice)
{
    // rcs_print("cartpos1: %f, %f, %f, %f, %f, %f\n", cartpos1->tran.x, cartpos1->tran.y, cartpos1->tran.z, cartpos1->a, cartpos1->b, cartpos1->c);
    // rcs_print("cartpos2: %f, %f, %f, %f, %f, %f\n", cartpos2->tran.x, cartpos2->tran.y, cartpos2->tran.z, cartpos2->a, cartpos2->b, cartpos2->c);
    // rcs_print("cartpos3: %f, %f, %f, %f, %f, %f\n", cartpos3->tran.x, cartpos3->tran.y, cartpos3->tran.z, cartpos3->a, cartpos3->b, cartpos3->c);
    PmCartesian vector1, vector2, crox, croy, z_vec, x_vec, y_vec, vector2_norm;
    PmRotationMatrix mat_conical, R_x, R_z, mat_tool_x, mat_tool_final;
    PmRpy conicalCenRpy_temp, pos3_RPY;
    pmCartCartSub(&cartpos2->tran, &cartpos1->tran, &vector1);
    pos3_RPY.y = cartpos3->a * PM_PI / 180.0;
    pos3_RPY.p = cartpos3->b * PM_PI / 180.0;
    pos3_RPY.r = cartpos3->c * PM_PI / 180.0;
    double x_tilt = *tilt * PM_PI / 180.0;
    double z_pan = *pan * PM_PI / 180.0;
    double s_x_tilt = sin(x_tilt);
    double c_x_tilt = cos(x_tilt);
    double s_z_pan = sin(z_pan);
    double c_z_pan = cos(z_pan);
    R_x.x.x = 1;
    R_x.y.x = 0;
    R_x.z.x = 0;
    R_x.x.y = 0;
    R_x.y.y = c_x_tilt;
    R_x.z.y = -s_x_tilt;
    R_x.x.z = 0;
    R_x.y.z = s_x_tilt;
    R_x.z.z = c_x_tilt;

    R_z.x.x = c_z_pan;
    R_z.y.x = -s_z_pan;
    R_z.z.x = 0;
    R_z.x.y = s_z_pan;
    R_z.y.y = c_z_pan;
    R_z.z.y = 0;
    R_z.x.z = 0;
    R_z.y.z = 0;
    R_z.z.z = 1;
    PmRotationMatrix tool_rotate_mat, tool_rotate_mat_x, tool_rotate_mat_final;
    PmHomogeneous hom_crd;
    if (*tipChoice == 0)
    {
        PmPose pos_end;
        PmRpy rpy_end;
        pos_end.tran = cartpos3->tran;
        rpy_end.y = pos3_RPY.y;
        rpy_end.p = pos3_RPY.p;
        rpy_end.r = pos3_RPY.r;
        pmRpyQuatConvert(&rpy_end, &pos_end.rot);
        PmPose pos_off;
        PmRpy rpy_off;
        pos_off.tran = zucmotConfig.tool_offset.tran;
        rpy_off.y = zucmotConfig.tool_offset.a * PM_PI / 180.0;
        rpy_off.p = zucmotConfig.tool_offset.b * PM_PI / 180.0;
        rpy_off.r = zucmotConfig.tool_offset.c * PM_PI / 180.0;
        pmRpyQuatConvert(&rpy_off, &pos_off.rot);
        PmPose pos_off_inv;
        pmPoseInv(&pos_off, &pos_off_inv);
        PmPose pos_crd;
        pmPosePoseMult(&pos_end, &pos_off_inv, &pos_crd);
        pmPoseHomConvert(&pos_crd, &hom_crd);
    }
    else if (*tipChoice == 1)
    {
        pmRpyMatConvert(&pos3_RPY, &hom_crd.rot);
        //rcs_print("tool_offset: %f, %f, %f\n", tool_rotate.y*180.0/PM_PI, tool_rotate.p*180.0/PM_PI, tool_rotate.r*180.0/PM_PI);
    }
    pmMatMatMult(&hom_crd.rot, &R_z, &mat_tool_x);
    pmMatMatMult(&mat_tool_x, &R_x, &mat_tool_final);
    // pmMatMatMult(&R_x,&mat_tool,&mat_tool_x);
    // pmMatMatMult(&R_z,&mat_tool_x,&mat_tool_final);
    vector2.x = mat_tool_final.z.x;
    vector2.y = mat_tool_final.z.y;
    vector2.z = mat_tool_final.z.z;
    //rcs_print("vector2: %f, %f, %f\n", vector2.x, vector2.y, vector2.z);
    pmCartUnit(&vector1, &z_vec);
    pmCartUnit(&vector2, &vector2_norm);
    if (fabs(z_vec.x) < E_EPSILON && fabs(z_vec.y) < E_EPSILON && fabs(z_vec.z) < E_EPSILON)
    {
        conicalCenRpy.r = 0;
        conicalCenRpy.p = 0;
        conicalCenRpy.y = 0;
        deviation = 0;
        usrOpeErrCode = -1;
        return -1;
    }
    pmCartCartCross(&vector1, &vector2, &crox);
    pmCartCartCross(&vector1, &crox, &croy);
    pmCartUnit(&crox, &x_vec);
    pmCartUnit(&croy, &y_vec);

    //rcs_print("vector2_norm: %f, %f, %f\n", vector2_norm.x, vector2_norm.y, vector2_norm.z);
    mat_conical.x.x = x_vec.x;
    mat_conical.y.x = y_vec.x;
    mat_conical.z.x = z_vec.x;
    mat_conical.x.y = x_vec.y;
    mat_conical.y.y = y_vec.y;
    mat_conical.z.y = z_vec.y;
    mat_conical.x.z = x_vec.z;
    mat_conical.y.z = y_vec.z;
    mat_conical.z.z = z_vec.z;
    pmMatRpyConvert(&mat_conical, &conicalCenRpy_temp);
    conicalCenRpy.r = conicalCenRpy_temp.y * 180 / PM_PI;
    conicalCenRpy.p = conicalCenRpy_temp.p * 180 / PM_PI;
    conicalCenRpy.y = conicalCenRpy_temp.r * 180 / PM_PI;
    double dot_vec = 0.0;
    if (fabs(z_vec.x - vector2_norm.x) < E_EPSILON && fabs(z_vec.y - vector2_norm.y) < E_EPSILON && fabs(z_vec.z - vector2_norm.z) < E_EPSILON)
    {
        deviation = 0;
    }
    else
    {
        dot_vec = (z_vec.x * vector2_norm.x + z_vec.y * vector2_norm.y + z_vec.z * vector2_norm.z) /
                  (pmSqrt(pmSq(z_vec.x) + pmSq(z_vec.y) + pmSq(z_vec.z)) * pmSqrt(pmSq(vector2_norm.x) + pmSq(vector2_norm.y) + pmSq(vector2_norm.z)));
        deviation = fabs(acos(dot_vec)) * 180 / PM_PI;
    }

    usrOpeErrCode = 0;
    // rcs_print("conicalCenRpy: %f, %f, %f\n", conicalCenRpy.r, conicalCenRpy.p, conicalCenRpy.y);
    // rcs_print("mat_conical.z: %f, %f, %f\n", mat_conical.z.x, mat_conical.z.y, mat_conical.z.z);
    // rcs_print("deviation: %f\n", deviation);
    return 0;
}

int zucTrajCalcUser(const unsigned char id,
                    const unsigned char saveType,
                    const unsigned char statusFlag,
                    const double* jntPos1,
                    const double* jntPos2,
                    const double* jntPos3)
{
    unsigned long curr_iflags = 0, curr_fflags = 0;
    ZucPose cartPos1 = {0.0}, cartPos2 = {0.0}, cartPos3 = {0.0};
    ZucPose flangeTool;
    ZucPose flangeUser;
    PmCartesian x0, y0, z0;
    double cos_theta /* , magB */;
    PmRotationMatrix rot;
    PmRpy rpy;
    ZERO_ZUC_POSE(flangeUser);
    flangeTool = zucmotConfig.tool_offset;

    rcs_print("[ zucTrajCalcUser ]: flangeTool =     %f, %f, %f\n", flangeTool.tran.x, flangeTool.tran.y, flangeTool.tran.z);
    auto kine = axisgroup::AxisGroupMgr::instance().get_axisgroup(0)->kine();
    if (fabs(jntPos1[0]) + fabs(jntPos1[1]) + fabs(jntPos1[2]) + fabs(jntPos1[3]) + fabs(jntPos1[4]) + fabs(jntPos1[5]) > 0.01)
        kine->fkine(RobJointVal(jntPos1), cartPos1, &flangeTool, &flangeUser, &zucmotConfig.base_offset);
    rcs_print("[ zucTrajCalcUser ]: CartPos1 is: %f, %f, %f\n", cartPos1.tran.x, cartPos1.tran.y, cartPos1.tran.z);
    if (fabs(jntPos2[0]) + fabs(jntPos2[1]) + fabs(jntPos2[2]) + fabs(jntPos2[3]) + fabs(jntPos2[4]) + fabs(jntPos2[5]) > 0.01)
        kine->fkine(RobJointVal(jntPos2), cartPos2, &flangeTool, &flangeUser, &zucmotConfig.base_offset);
    rcs_print("[ zucTrajCalcUser ]: CartPos2 is: %f, %f, %f\n", cartPos2.tran.x, cartPos2.tran.y, cartPos2.tran.z);
    if (fabs(jntPos3[0]) + fabs(jntPos3[1]) + fabs(jntPos3[2]) + fabs(jntPos3[3]) + fabs(jntPos3[4]) + fabs(jntPos3[5]) > 0.01)
        kine->fkine(RobJointVal(jntPos3), cartPos3, &flangeTool, &flangeUser, &zucmotConfig.base_offset);
    rcs_print("[ zucTrajCalcUser ]: CartPos3 is: %f, %f, %f\n", cartPos3.tran.x, cartPos3.tran.y, cartPos3.tran.z);
    usrOpeErrCode = 0;
    ZERO_ZUC_POSE(utilCartPos);
    if (saveType || id == 0)  // 执行MoveZ、MoveZS指令时 APP生成的jks会根据指令参数临时修改世界坐标系。
    {
        int ret_zero_x0 = 0, ret_zero_y0 = 0;
        utilCartPos.tran = cartPos1.tran;
        pmCartCartSub(&cartPos2.tran, &cartPos1.tran, &x0);
        ret_zero_x0 = pmCheckCartZero(&x0);
        pmCartUnitEq(&x0);
        pmCartCartSub(&cartPos3.tran, &cartPos2.tran, &y0);
        ret_zero_y0 = pmCheckCartZero(&y0);
        pmCartUnitEq(&y0);
        pmCartCartDot(&x0, &y0, &cos_theta);
        //余弦值判断向量v12和v23是否共线，坐标值求和判断是否为零向量
        if ((1 - fabs(cos_theta)) < 0.001 || ret_zero_x0 || ret_zero_y0)
        {
            zucOperatorError(USER_CALIBRATION_FAILED,
                             "user frame calibration failed! cos_theta: %f, Vector12: <%f %f %f>, Vector23: <%f %f %f> \n",
                             cos_theta,
                             x0.x,
                             x0.y,
                             x0.z,
                             y0.x,
                             y0.y,
                             y0.z);
            ZERO_ZUC_POSE(utilCartPos);
            return -1;
        }

        pmCartCartCross(&x0, &y0, &z0);
        pmCartUnitEq(&z0);
        pmCartCartCross(&z0, &x0, &y0);
        rot.x = x0;
        rot.y = y0;
        rot.z = z0;
        pmMatRpyConvert(&rot, &rpy);
        utilCartPos.a = rpy.y * 180 / PM_PI;
        utilCartPos.b = rpy.p * 180 / PM_PI;
        utilCartPos.c = rpy.r * 180 / PM_PI;
    }
    iniSaveUserCalibration(id, statusFlag, cartPos1, cartPos2, cartPos3, jntPos1, jntPos2, jntPos3);
    return 0;
}

int zucCalibDHParams(DHParam dhParamError)
{
    for (int i = 0; i < zucStatus->motion.robot_joint_num; i++)
    {
        zucTaskDHParam.alpha[i] += dhParamError.alpha[i];
        zucTaskDHParam.a[i] += dhParamError.a[i];
        zucTaskDHParam.d[i] += dhParamError.d[i];
        zucTaskDHParam.joint_homeoff[i] += dhParamError.joint_homeoff[i];
    }

    setInterpDHParam(zucTaskDHParam);
    iniSaveDHParam(zucStatus->motion.robot_serial_num, zucTaskDHParam, zuc_inifile);
    zucmotCommand.command = ZUCMOT_SET_DH_PARAM;
    zucmotCommand.calibdhparams.dhParam = zucTaskDHParam;
    for (int i = 0; i < zucStatus->motion.robot_joint_num; i++)
    {
        dh_a[i] = zucTaskDHParam.a[i] / 1000.0;
        dh_d[i] = zucTaskDHParam.d[i] / 1000.0;
        dh_alpha[i] = zucTaskDHParam.alpha[i] * PM_PI / 180.0;
    }
    // auto cmd = std::make_shared<CmdSetDhParam>(0, dhParam);
    // return mot::motion_cmd_call(0, cmd);
    DynPara_init_toppra(toolParameters, install_angle, dynParameters, dh_a, dh_d, dh_alpha);
    return 0;
}

int zucTrajSetTermCond(int cond, double tolerance)
{
    last_tc_blend_r = cur_tc_blend_r;
    cur_tc_blend_r = tolerance;  // update cur_tc_blend_r for toppra

    auto cmd = std::make_shared<CmdSetTermCond>(0, cond, tolerance);
    return mot::realtime_cmd_call(cmd);
}

int cal_using_abc(ZucPose start_cartpos, ZucPose end, double vel, double acc, double ori_vel, double ori_acc, int* using_abc, double* tmag_abc)
{
    int ret = 0;
    double delta_x = fabs(start_cartpos.tran.x - end.tran.x);
    double delta_y = fabs(start_cartpos.tran.y - end.tran.y);
    double delta_z = fabs(start_cartpos.tran.z - end.tran.z);
    double delta_rx = fabs(start_cartpos.a - end.a);
    double delta_ry = fabs(start_cartpos.b - end.b);
    double delta_rz = fabs(start_cartpos.c - end.c);
    int repeat_position = 0;
    int repeat_ori = 0;
    if ((delta_x < SMALL) && (delta_y < SMALL) && (delta_z < SMALL))
    {
        repeat_position = 1;
    }
    if ((delta_rx < SMALL) && (delta_ry < SMALL) && (delta_rz < SMALL))
    {
        repeat_ori = 1;
    }
    double delta_cart_len = sqrt(delta_x * delta_x + delta_y * delta_y + delta_z * delta_z);
    double delta_ori_len = 0.0;
    if (!repeat_ori)
    {
        //get angle
        PmRpy rpyTarget, rpyCurr;
        PmQuaternion quatTarget, quatCurr, quatDelt;
        rpyCurr.y = start_cartpos.a * PM_PI / 180.0;
        rpyCurr.p = start_cartpos.b * PM_PI / 180.0;
        rpyCurr.r = start_cartpos.c * PM_PI / 180.0;
        rpyTarget.y = zucmotCommand.trajlinearmov.pos.a * PM_PI / 180.0;
        rpyTarget.p = zucmotCommand.trajlinearmov.pos.b * PM_PI / 180.0;
        rpyTarget.r = zucmotCommand.trajlinearmov.pos.c * PM_PI / 180.0;

        pmRpyQuatConvert(&rpyTarget, &quatTarget);
        pmRpyQuatConvert(&rpyCurr, &quatCurr);
        pmQuatInv(&quatCurr, &quatCurr);
        pmQuatQuatMult(&quatTarget, &quatCurr, &quatDelt);
        if (quatDelt.s > 1.0)
            quatDelt.s = 1.0;
        else if (quatDelt.s < -1.0)
            quatDelt.s = -1.0;
        *tmag_abc = fabs(2 * acos(quatDelt.s) * 180.0 / PM_PI);
        delta_ori_len = *tmag_abc;
    }
    if (repeat_position && repeat_ori)
    {
        ret = 1;  //repeat point
    }
    else if (repeat_position)
    {
        *using_abc = 1;
    }
    else if (repeat_ori)
    {
        *using_abc = 0;
    }
    else
    {
        double triangle_vel = sqrt(acc * delta_cart_len);
        double t_for_pos = 0.0;
        if (triangle_vel < vel)
        {
            t_for_pos = 2.0 * delta_cart_len / triangle_vel;
        }
        else
        {
            double t_acc = vel / acc;
            double t_dec = t_acc;
            double t_const = (delta_cart_len - (t_acc * vel)) / vel;
            t_for_pos = t_acc + t_const + t_dec;
        }
        rtapi_print(" acc = %f, vel = %f, triangle_vel = %f, t_for_pos = %f \n", acc, vel, triangle_vel, t_for_pos);
        rtapi_print(" -------------------------- tmag_abc = %f, delta_cart_len = %f -------------------------- \n", *tmag_abc, delta_cart_len);
        double triangle_vel_ori = sqrt(delta_ori_len * ori_acc);
        double t_for_ori = 0.0;
        if (triangle_vel_ori < ori_vel)
        {
            t_for_ori = 2.0 * delta_ori_len / triangle_vel_ori;
        }
        else
        {
            double t_acc = ori_vel / ori_acc;
            double t_dec = t_acc;
            double t_const = (delta_ori_len - (t_acc * ori_vel)) / ori_vel;
            t_for_ori = t_acc + t_const + t_dec;
        }
        rtapi_print(" ori_acc = %f, ori_vel = %f, triangle_vel_ori = %f, t_for_ori = %f \n", ori_acc, ori_vel, triangle_vel_ori, t_for_ori);

        if (t_for_ori >= t_for_pos)
        {
            *using_abc = 1;
        }
        else
        {
            *using_abc = 0;
        }
    }
    return ret;
}

int zucTrajLinearMove(ZucPose end,
                      int type,
                      double vel,
                      double ini_maxvel,
                      double acc,
                      double jerk,
                      double ori_vel,
                      double ori_acc,
                      int indexrotary,
                      int using_abc,
                      int di_type,
                      int di_index,
                      int di_state,
                      int planner_type)
{
    if (zucmotStatus.protective_stop == 1 && zucStatus->task.interpState == ZUC_TASK_INTERP_IDLE)
    {
        rcs_print("Cannot do linear move when in protective_stop status\n");
        return -1;
    }

#ifdef ISNAN_TRAP
    if (std::isnan(end.tran.x) || std::isnan(end.tran.y) || std::isnan(end.tran.z) || std::isnan(end.a) || std::isnan(end.b) || std::isnan(end.c) ||
        std::isnan(end.u) || std::isnan(end.v) || std::isnan(end.w))
    {
        printf("std::isnan error in zucTrajLinearMove()\n");
        return 0;  // ignore it for now, just don't send it
    }
#endif

    zuc_traj_linearmov_t trajlinearmov = {};
    int id = TrajConfig.MotionId;

    // rtapi_print("ZUCMOT_SET_LINE ---> end = %f %f %f %f %f %f\n", end.tran.x, end.tran.y, end.tran.z, end.a, end.b, end.c);
    trajlinearmov.pos = end;
    trajlinearmov.motion_type = type;
    trajlinearmov.vel = vel;
    trajlinearmov.ini_maxvel = ini_maxvel;
    trajlinearmov.acc = acc;
    trajlinearmov.turn = indexrotary;
    trajlinearmov.jerk = jerk;
    trajlinearmov.ori_vel = ori_vel;
    trajlinearmov.ori_acc = ori_acc;
    trajlinearmov.using_abc_vel = using_abc;
    trajlinearmov.di_type = di_type;
    trajlinearmov.di_state = di_state;
    trajlinearmov.di_index = di_index;
    trajlinearmov.planner_type = planner_type;

    if ((!userset_use_toppra || kinematic_error) && trajlinearmov.planner_type == TOPP_PLANNER)
    {
        rtapi_print("******************************** [unable use toppra] ---> set planner_type = T_planner *******************************\n");
        trajlinearmov.planner_type = T_PLANNER;  //配置文件中没有开启时间最优时默认使用T规划
    }
    if (trajlinearmov.planner_type == S_PLANNER && trajlinearmov.jerk < 1e-6)
    {
        trajlinearmov.jerk = 0.0;
        trajlinearmov.planner_type = T_PLANNER;  //用户设置规划器为S规划，但是jerk值为0时，切换规划器为T规划；
    }

    tc_motion_type_t motion_type = TC_LINEAR;

    double safety_func_vel = get_vel_by_stopdisttime(jerk, motion_type, trajlinearmov.planner_type);
    if (safety_func_vel < ini_maxvel)
    {
        trajlinearmov.vel = safety_func_vel;
        trajlinearmov.ini_maxvel = safety_func_vel;
    }
    clock_t start_t, finish_t;
    double duration;
    if (first_command)
    {
        get_last_pos();
        first_command = 0;
    }
    ZucPose start_cartpos;
    auto kine = axisgroup::AxisGroupMgr::instance().get_axisgroup(0)->kine();
    //首先进行一次运动学正解，获得运动段起始点的笛卡尔坐标并更新逆解标志位
    kine->fkine(RobJointVal(cur_tc_goal_jointpos), start_cartpos, &(zucmotConfig.tool_offset), &(zucmotConfig.user_offset), &(zucmotConfig.base_offset));

    RobJointVal jpos(cur_tc_goal_jointpos);
    kine::KineItf::kine_Conf conf;
    auto inv_ret = kine->ikine(end, jpos, conf, &(zucmotConfig.tool_offset), &(zucmotConfig.user_offset), &(zucmotConfig.base_offset));
    jpos.array_value(cur_tc_goal_jointpos);
    if (inv_ret != 0)
    {
        kinematic_error = 1;
    }
    else
    {
        update_related_params2();
    }

    auto cmd = std::make_shared<CmdSetLine>(0, id, motion_type, trajlinearmov);
    return mot::realtime_cmd_call(cmd);
}

int zucTrajMovs(const double* a0, const double* a1, const double* a2, const double* a3, double time)
{
    int i = 0;
    zuc_traj_movs_t trajmovs = {};
    int id = TrajConfig.MotionId;
    for (i = 0; i < 6; i++)
    {
        trajmovs.a0[i] = a0[i];
        trajmovs.a1[i] = a1[i];
        trajmovs.a2[i] = a2[i];
        trajmovs.a3[i] = a3[i];
    }
    trajmovs.time = time;
    kinematic_error = 1;

    auto cmd = std::make_shared<CmdSetMovs>(0, id, trajmovs);
    return mot::realtime_cmd_call(cmd);
}

int zucTrajMovc(ZucPose mid,
                ZucPose end,
                double vel,
                double ini_maxvel,
                double acc,
                double jerk,
                int using_abc,
                double circle_count,
                int di_type,
                int di_index,
                int di_state,
                int planner_type,
                int circlemode)
{
    if (zucmotStatus.protective_stop == 1 && zucStatus->task.interpState == ZUC_TASK_INTERP_IDLE)
    {
        rcs_print("Cannot do movc move when in protective_stop status\n");
        return -1;
    }
#ifdef ISNAN_TRAP
    if (std::isnan(mid.tran.x) || std::isnan(mid.tran.y) || std::isnan(mid.tran.z) || std::isnan(mid.a) || std::isnan(mid.b) || std::isnan(mid.c) ||
        std::isnan(mid.u) || std::isnan(mid.v) || std::isnan(mid.w))
    {
        printf("std::isnan error in zucTrajLinearMove()\n");
        return 0;  // ignore it for now, just don't send it
    }
    if (std::isnan(end.tran.x) || std::isnan(end.tran.y) || std::isnan(end.tran.z) || std::isnan(end.a) || std::isnan(end.b) || std::isnan(end.c) ||
        std::isnan(end.u) || std::isnan(end.v) || std::isnan(end.w))
    {
        printf("std::isnan error in zucTrajLinearMove()\n");
        return 0;  // ignore it for now, just don't send it
    }
#endif

    zuc_traj_movc_t trajmovc = {};
    int id = TrajConfig.MotionId;

    trajmovc.movcMid = mid;
    trajmovc.movcEnd = end;
    trajmovc.vel = vel;
    trajmovc.ini_maxvel = ini_maxvel;
    trajmovc.acc = acc;
    trajmovc.jerk = jerk;
    trajmovc.using_abc_vel = using_abc;
    trajmovc.circle_count = circle_count;
    trajmovc.di_type = di_type;
    trajmovc.di_state = di_state;
    trajmovc.di_index = di_index;
    trajmovc.circlemode = circlemode;

    trajmovc.planner_type = planner_type;
    if (((!userset_use_toppra || kinematic_error) && trajmovc.planner_type == TOPP_PLANNER) || trajmovc.circle_count > 1e-4 || circlemode != 0)
    {
        rtapi_print(
            "******************************** [unable use toppra] ---> planner_type = T_PLANNER, kinematic_error = %d, circle_count = %f, circlemode = %d "
            "*******************************\n",
            kinematic_error,
            trajmovc.circle_count,
            circlemode);
        trajmovc.planner_type = T_PLANNER;  //配置文件中没有开启时间最优或整圆运动时默认使用T规划
    }
    if (trajmovc.planner_type == S_PLANNER && trajmovc.jerk < 1e-6)
    {
        trajmovc.jerk = 0.0;
        trajmovc.planner_type = T_PLANNER;  //用户设置规划器为S规划，但是jerk值为0时，切换规划器为T规划；
    }
    zucmotCommand.motion_type = TC_MOVC;

    double safety_func_vel = get_vel_by_stopdisttime(jerk, zucmotCommand.motion_type, trajmovc.planner_type);
    if (safety_func_vel < ini_maxvel)
    {
        trajmovc.vel = safety_func_vel;
        trajmovc.ini_maxvel = safety_func_vel;
    }
    clock_t start_t, finish_t;
    double duration;
    if (first_command)
    {
        get_last_pos();
        first_command = 0;
    }

    if (trajmovc.planner_type == TOPP_PLANNER)
    {
        if (trajmovc.vel > tcp_vel_limit_toppra)
        {
            trajmovc.vel = tcp_vel_limit_toppra;
            trajmovc.ini_maxvel = tcp_vel_limit_toppra;
        }
        start_t = clock();  //计时
        rtapi_print("[movc 1]last_tc_blend_r = %f\n", last_tc_blend_r);
        if (movc_get_toppra_params() != 0)
        {
            trajmovc.planner_type = T_PLANNER;
            rtapi_print("[movc_get_toppra_params != 0]   --->  SET planner_type = T_PLANNER\n");
        }
        finish_t = clock();
        duration = (double)(finish_t - start_t) / CLOCKS_PER_SEC;
        rtapi_print(" ======================================================================= movc_get_toppra_params = %fms "
                    "======================================================================= \n",
                    duration * 1000);
        if (!zucmotCommand.already_inpos)
        {
            update_related_params();
        }

        if (zucmotCommand.already_inpos)
        {
            cur_tc_blend_r = 0.0;
            last_tc_blend_r = 0.0;
            rtapi_print(" --------------------------------------------------- movc zucmotCommand.already_inpos, last_tc_blend_r = %f "
                        "--------------------------------------------------- \n",
                        last_tc_blend_r);
            // return 0;
        }
    }

    if (trajmovc.planner_type != TOPP_PLANNER)
    {
        KINEMATICS_FORWARD_FLAGS toppra_fflags = 0;
        KINEMATICS_INVERSE_FLAGS toppra_iflags = 0;
        //首先进行一次运动学正解，获得运动段起始点的笛卡尔坐标并更新逆解标志位
        ZucPose start_cartpos;
        kinematicsForward(&(zucmotConfig.tool_offset),
                          cur_tc_goal_jointpos,
                          &start_cartpos,
                          &toppra_fflags,
                          &toppra_iflags,
                          &(zucmotConfig.base_offset),
                          &(zucmotConfig.user_offset));
        int inv_ret = kinematicsInverse(
            &zucmotConfig.tool_offset, &end, cur_tc_goal_jointpos, &toppra_iflags, &toppra_fflags, &zucmotConfig.base_offset, &zucmotConfig.user_offset);
        // rtapi_print(" ------------------------------------------------------------------------------------ cur_tc_goal_joints = %f %f %f %f %f %f \n",
        //             cur_tc_goal_jointpos[0],
        //             cur_tc_goal_jointpos[1],
        //             cur_tc_goal_jointpos[2],
        //             cur_tc_goal_jointpos[3],
        //             cur_tc_goal_jointpos[4],
        //             cur_tc_goal_jointpos[5]);
        if (inv_ret != 0 || trajmovc.circle_count > 1e-4)
        {
            kinematic_error = 1;
        }
        else
        {
            update_related_params2();
        }
    }

    // auto cmd = std::make_shared<CmdSetMovc>(0, id, trajmovc);
    // return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucTrajCircularMove(ZucPose end, PM_CARTESIAN center, PM_CARTESIAN normal, int turn, int type, double vel, double ini_maxvel, double acc, double jerk)
{
    if (zucmotStatus.protective_stop == 1 && zucStatus->task.interpState == ZUC_TASK_INTERP_IDLE)
    {
        rcs_print("Cannot do circular move when in protective_stop status\n");
        return -1;
    }

#ifdef ISNAN_TRAP
    if (std::isnan(end.tran.x) || std::isnan(end.tran.y) || std::isnan(end.tran.z) || std::isnan(end.a) || std::isnan(end.b) || std::isnan(end.c) ||
        std::isnan(end.u) || std::isnan(end.v) || std::isnan(end.w) || std::isnan(center.x) || std::isnan(center.y) || std::isnan(center.z) ||
        std::isnan(normal.x) || std::isnan(normal.y) || std::isnan(normal.z))
    {
        printf("std::isnan error in zucTrajCircularMove()\n");
        return 0;  // ignore it for now, just don't send it
    }
#endif

    zuc_traj_circular_mov_t trajcircularmove;
    int id = TrajConfig.MotionId;

    trajcircularmove.pos = end;
    trajcircularmove.motion_type = type;
    trajcircularmove.center.x = center.x;
    trajcircularmove.center.y = center.y;
    trajcircularmove.center.z = center.z;
    trajcircularmove.normal.x = normal.x;
    trajcircularmove.normal.y = normal.y;
    trajcircularmove.normal.z = normal.z;
    trajcircularmove.turn = turn;
    trajcircularmove.vel = vel;
    trajcircularmove.ini_maxvel = ini_maxvel;
    trajcircularmove.acc = acc;
    trajcircularmove.jerk = jerk;

    auto cmd = std::make_shared<CmdSetCircle>(0, id, trajcircularmove);
    return mot::realtime_cmd_call(cmd);
}

int zucTrajJointMove(ZucPose end,
                     int type,
                     double velPercent,
                     double ini_maxvel,
                     double acc,
                     double jerk,
                     int indexrotary,
                     int di_type,
                     int di_index,
                     int di_state,
                     int jointNum,
                     int planner_type)
{
    if (zucmotStatus.protective_stop == 1 && zucStatus->task.interpState == ZUC_TASK_INTERP_IDLE)
    {
        rcs_print("Cannot do joint move when in protective_stop status\n");
        return -1;
    }

#ifdef ISNAN_TRAP
    if (std::isnan(end.tran.x) || std::isnan(end.tran.y) || std::isnan(end.tran.z) || std::isnan(end.a) || std::isnan(end.b) || std::isnan(end.c) ||
        std::isnan(end.u) || std::isnan(end.v) || std::isnan(end.w))
    {
        printf("std::isnan error in zucTrajJointMove()\n");
        return 0;  // ignore it for now, just don't send it
    }
#endif

    int id = TrajConfig.MotionId;
    zuc_traj_jointmov_t trajjointmov = {};
    trajjointmov.pos = end;  // Note that, here the position is in Joint Space
    trajjointmov.motion_type = type;
    trajjointmov.vel = velPercent;
    trajjointmov.ini_maxvel = ini_maxvel;
    trajjointmov.acc = acc;
    trajjointmov.turn = indexrotary;
    trajjointmov.arcBlendEnable = 0; /*added here, by zxqi to avoid blending*/
    trajjointmov.jerk = jerk;
    trajjointmov.di_type = di_type;
    trajjointmov.di_state = di_state;
    trajjointmov.di_index = di_index;
    trajjointmov.planner_type = planner_type;

    if ((!userset_use_toppra || kinematic_error) && trajjointmov.planner_type == TOPP_PLANNER)
    {
        rtapi_print("******************************** [unable use toppra] ---> planner_type = T_PLANNER "
                    "*******************************\n");
        trajjointmov.planner_type = T_PLANNER;  //配置文件中没有开启时间最优或整圆运动时默认使用T规划
    }
    if (trajjointmov.planner_type == S_PLANNER && trajjointmov.jerk < 1e-6)
    {
        trajjointmov.jerk = 0.0;
        trajjointmov.planner_type = T_PLANNER;  //用户设置规划器为S规划，但是jerk值为0时，切换规划器为T规划；
    }

    int motion_type = TC_JOINT;
    double safety_func_vel = get_vel_by_stopdisttime(jerk, motion_type, trajjointmov.planner_type);
    if (safety_func_vel < ini_maxvel)
    {
        trajjointmov.vel = safety_func_vel;
        trajjointmov.ini_maxvel = safety_func_vel;
    }
    // clock_t start_t, finish_t;
    // double duration;
    //TOPPRA_TODO : 不使用toppra运动段也要对last_tc_goal_joint和转接半径进行更新
    // init_DYN();
    if (first_command)
    {
        get_last_pos();
        first_command = 0;
    }

    if (trajjointmov.planner_type == TOPP_PLANNER)
    {
        // rtapi_print(" **************************** start movj_get_toppra_params, jointNum = %d, end = %f %f %f %f %f %f\n", jointNum, end.tran.x, end.tran.y, end.tran.z, end.a, end.b, end.c);
        // csaps::DoubleArray xidata(5);
        // xidata << DoubleArray::LinSpaced(5, 0, 1);
        // rtapi_print("xidata = %f %f %f %f %f\n", xidata(0), xidata(1), xidata(2), xidata(3), xidata(4));
        // start_t = clock();  //计时
        if (movj_get_toppra_params() != 0)
        {
            trajjointmov.planner_type = T_PLANNER;
            rtapi_print("[movj_get_toppra_params != 0]   --->   [planner_type = T_PLANNER]\n");
        }
        // finish_t = clock();
        // duration = (double)(finish_t - start_t) / CLOCKS_PER_SEC;
        // rtapi_print(
        //     " ****************************************************** movj_get_toppra_params = %fms ****************************************************** \n",
        //     duration * 1000);

        // rtapi_print("end movj_get_toppra_params\n");
        /* update last_tc_goal_jointpos and last_tc_blend_r */
        // rtapi_print("before update, last_tc_goal_jointpos = %f %f %f %f %f %f\n", last_tc_goal_jointpos[0], last_tc_goal_jointpos[1], last_tc_goal_jointpos[2], last_tc_goal_jointpos[3], last_tc_goal_jointpos[4],
        //                 last_tc_goal_jointpos[5]);
        if (!zucmotCommand.already_inpos)
        {
            update_related_params();
        }
        if (zucmotCommand.already_inpos)
        {
            cur_tc_blend_r = 0.0;
            last_tc_blend_r = 0.0;
            rtapi_print(
                " --------------------------------------------------- zucmotCommand.already_inpos --------------------------------------------------- \n");
            // return 0;
        }
        // rtapi_print("after update, last_tc_goal_jointpos = %f %f %f %f %f %f\n", last_tc_goal_jointpos[0], last_tc_goal_jointpos[1], last_tc_goal_jointpos[2], last_tc_goal_jointpos[3], last_tc_goal_jointpos[4],
        //                 last_tc_goal_jointpos[5]);
    }
    // rtapi_print(" ============================================= Finish zucTrajJointMove ============================================= \n");
    if (trajjointmov.planner_type != TOPP_PLANNER)
    {
        cur_tc_goal_jointpos[0] = end.tran.x;
        cur_tc_goal_jointpos[1] = end.tran.y;
        cur_tc_goal_jointpos[2] = end.tran.z;
        cur_tc_goal_jointpos[3] = end.a;
        cur_tc_goal_jointpos[4] = end.b;
        cur_tc_goal_jointpos[5] = end.c;
        update_related_params2();
    }
    kinematic_error = 0;

    auto cmd = std::make_shared<CmdSetJointLine>(0, id, motion_type, trajjointmov);
    return mot::realtime_cmd_call(cmd);
}

int get_toppra_related_params(const int discrete_num, ZucJPose* jpos_for_fit, ZucJPose* blend_jpos_for_fit)
{
    zucmotCommand.blend_jpos_num = lasttc_left_len + zucmotCommand.cur_last_blend_len;
    // rtapi_print("lasttc_left_len = %d, cur_last_blend_len = %d, blend_jpos_num = %d\n", lasttc_left_len, zucmotCommand.cur_last_blend_len,
    // zucmotCommand.blend_jpos_num);
    zucmotCommand.jpos_len = discrete_num;
    /* start to fit with csaps */

    double start_path_data = last_final_pathp;
    get_pathvalue_coefs_pycubic(discrete_num, jpos_for_fit);

    ConstraintsToppra cons_toppra;
    // zucmotCommand.cons_toppra.req_vel[0] = 0;
    set_constraints_toppra(zucmotCommand.cons_toppra);
    // rtapi_print("cons_toppra.vel_lim = %f %f\n", zucmotCommand.cons_toppra.vel_lim[0][0], zucmotCommand.cons_toppra.vel_lim[0][1]);
    get_path_information(discrete_num, zucmotCommand.cons_toppra);

    if ((last_tc_blend_r > SMALL) && (zucmotCommand.cur_last_blend_len > 0) && (zucmotCommand.blend_jpos_num > 6))  // TOPPRA_TODO: >6 ??
    {
        /* 对转接段进行拟合,计算转接段的系数 */
        // rtapi_print(" ========================== last_tc_blend_r = %f,  zucmotCommand.cur_last_blend_len = %d,  zucmotCommand.blend_jpos_num = %d "
        //             "========================== \n",
        //             last_tc_blend_r,
        //             zucmotCommand.cur_last_blend_len,
        //             zucmotCommand.blend_jpos_num);
        get_blend_pathvalue_coefs_csaps(zucmotCommand.blend_jpos_num, start_path_data, blend_jpos_for_fit);
        get_blend_path_information(zucmotCommand.blend_jpos_num + 2 + REFIT_NUM, zucmotCommand.cons_toppra);
        // rtapi_print(" ========================== finish get blend pos, blend_jpos_num = %d ========================== \n", zucmotCommand.blend_jpos_num);
    }
    else
    {
        zucmotCommand.cur_last_blend_len = 0;
        zucmotCommand.cur_blend_len = discrete_num - 1;
    }
    // rtapi_print(
    //     " =================>>>>>>>>>>>>>>>>>>>>>>>>> zucmotCommand.cur_blend_len = %d, discrete_num = %d \n", zucmotCommand.cur_blend_len, discrete_num);
    lasttc_left_len = discrete_num - zucmotCommand.cur_blend_len;  // update lasttc_left_len;
    zucmotCommand.cur_blend_left = lasttc_left_len;
    return 0;
}

void get_lasttc_blend_jpos(ZucJPose* blend_jpos_for_fit)
{
    BlendJpos blendjpos;
    int get_front_ret = -1;
    for (int i = 0; i < lasttc_left_len; i++)
    {
        get_front_ret = GetBlendFront(&blendjpos);
        if (get_front_ret != 0)
        {
            last_tc_blend_r = 0;
            return;
        }
        else
        {
            for (int j = 0; j < zucStatus->motion.robot_joint_num; j++) { blend_jpos_for_fit[i].jpos[j] = blendjpos.blendjpos[j]; }
        }
    }
}

void update_related_params()
{
    last_tc_blend_r = cur_tc_blend_r;
    lasttc_jointNum = zucmotCommand.joint;
    int index = 0;
    for (int i = 0; i < zucStatus->motion.robot_joint_num; i++)
    {
        last_tc_goal_jointpos[i] = cur_tc_goal_jointpos[i];
        last_tc_blend_jointpos[i] = cur_tc_blend_jointpos[i];

        if (cur_tc_blend_r > SMALL)
        {
            for (int j = REFIT_NUM; j > 0; j--)
            {
                index = zucmotCommand.cur_blend_len - j;
                last_tc_refit_jpos[REFIT_NUM - j][i] = zucmotCommand.path_info[index].pathq_coefs[i][3];
            }
        }
        else
        {
            for (int j = REFIT_NUM; j > 0; j--) { last_tc_refit_jpos[REFIT_NUM - j][i] = cur_tc_blend_jointpos[i]; }
        }

        last_tc_blend_jointpos_sec[i] = cur_tc_blend_jointpos_sec[i];
    }
    index = zucmotCommand.cur_blend_len;  // toppra changed here
}

double get_vel_by_stopdisttime(double jerk, int motion_type, int planner_type)
{
    double ret_vel = 10000.0;
    double v_time_limit = 10000.0;
    double v_dist_limit = 10000.0;
    double max_acc = 10000.0;
    switch (motion_type)
    {
    case TC_JOINT:
        max_acc = (720.0 < JointConfig[0].MaxAccel ? 720.0 : JointConfig[0].MaxAccel);  //需要改进，后面加速度值不一定被限制在720以内
        break;
    case TC_LINEAR:
        //如果规划主轴的确定是在此之前确定的，那么这里需要根据规划主轴的选择情况确定最大加速度值
        if (zucmotCommand.trajlinearmov.using_abc_vel)
        {
            max_acc = (720.0 < AxisConfig[3].MaxAccel ? 720.0 : AxisConfig[3].MaxAccel);
        }
        else
        {
            max_acc = (6000.0 < AxisConfig[0].MaxAccel ? 6000.0 : AxisConfig[0].MaxAccel);
        }

        break;
    case TC_MOVC:
        max_acc = (6000.0 < AxisConfig[0].MaxAccel ? 6000.0 : AxisConfig[0].MaxAccel);
        break;
    default:
        break;
    }
    if (planner_type != S_PLANNER)
    {
        if (planner_type == TOPP_PLANNER)
        {
            double real_lim_time = zucmotConfig.safetyCfg.stoppingTime - zucmotConfig.trajCycleTime * userset_filter_size;
            if (real_lim_time < 0.0)
            {
                real_lim_time = zucmotConfig.safetyCfg.stoppingTime;
                rtapi_print("[get_vel_by_stopdisttime] : real_lim_time < 0.0, stoppingTime = %f, filter_size = %d\n",
                            zucmotConfig.safetyCfg.stoppingTime,
                            userset_filter_size);
                userset_filter_size = 0;
            }
            v_time_limit = max_acc * real_lim_time;
        }
        else
        {
            //T planner
            v_time_limit = max_acc * zucmotConfig.safetyCfg.stoppingTime;
        }
        if (motion_type == TC_MOVC || motion_type == TC_LINEAR)
        {
            v_dist_limit = sqrt(2 * max_acc * zucmotConfig.safetyCfg.stoppingDist);
        }
    }
    else
    {
        //S planner
        //calculate vel constrainted by stop time
        double t_zero_to_maxacc = max_acc / jerk;
        double t_maxacc_to_zero = t_zero_to_maxacc;
        double t_const_acc = 0.0;
        if ((t_zero_to_maxacc + t_maxacc_to_zero) >= zucmotConfig.safetyCfg.stoppingTime)
        {
            //no const acc : triangle are
            v_time_limit = 0.25 * jerk * zucmotConfig.safetyCfg.stoppingTime * zucmotConfig.safetyCfg.stoppingTime;
        }
        else
        {
            //has const acc: trapezoid area
            t_const_acc = zucmotConfig.safetyCfg.stoppingTime - (t_zero_to_maxacc + t_maxacc_to_zero);
            v_time_limit = 0.5 * max_acc * (zucmotConfig.safetyCfg.stoppingTime + t_const_acc);
        }
        //calculate vel constrainted by stop distant
        double delta_s_maxacc_triangle = max_acc * t_zero_to_maxacc * t_zero_to_maxacc;
        if (delta_s_maxacc_triangle >= zucmotConfig.safetyCfg.stoppingDist)
        {
            //no const acc
            double temp_val = zucmotConfig.safetyCfg.stoppingDist / jerk;
            v_dist_limit = jerk * pow(temp_val, 2.0 / 3.0);
        }
        else
        {
            //has const acc
            double v_1 = 0.5 * jerk * t_zero_to_maxacc * t_zero_to_maxacc;
            double coef_a = 0.5 * max_acc;
            double coef_b = v_1 + max_acc * t_zero_to_maxacc;
            double coef_c = 0.5 * max_acc * t_zero_to_maxacc * t_zero_to_maxacc + v_1 * t_zero_to_maxacc - zucmotConfig.safetyCfg.stoppingDist;
            double delta_val = coef_b * coef_b - 4 * coef_a * coef_c;
            if (delta_val < 0.0)
            {
                //无解，按照三角算
                v_dist_limit = max_acc * max_acc / jerk;
            }
            else
            {
                double t_2 = (-coef_b + sqrt(delta_val)) / (2 * coef_a);
                if (t_2 < 0.0)
                {
                    v_dist_limit = max_acc * max_acc / jerk;
                }
                else
                {
                    v_dist_limit = max_acc * t_2 + v_1;
                }
            }
        }
    }
    // rtapi_print(" ======================= >> v_dist_limit = %f, v_time_limit = %f\n", v_dist_limit, v_time_limit);
    ret_vel = fmin(v_dist_limit, v_time_limit);
    return ret_vel;
}

void update_related_params2()
{
    last_tc_blend_r = 0.0;
    cur_tc_blend_r = 0.0;
    last_final_pathp = 0.0;
    lasttc_left_len = 0;
    lasttc_jointNum = zucmotCommand.joint;

    for (int i = 0; i < zucStatus->motion.robot_joint_num; i++)
    {
        last_tc_goal_jointpos[i] = cur_tc_goal_jointpos[i];
        last_tc_blend_jointpos[i] = cur_tc_goal_jointpos[i];
        last_tc_blend_jointpos_sec[i] = cur_tc_goal_jointpos[i];
        cur_tc_blend_jointpos[i] = cur_tc_goal_jointpos[i];
        cur_tc_blend_jointpos_sec[i] = cur_tc_goal_jointpos[i];

        for (int j = REFIT_NUM; j > 0; j--) { last_tc_refit_jpos[REFIT_NUM - j][i] = cur_tc_goal_jointpos[i]; }
    }
}

void get_last_pos()
{
    // rtapi_print(" ************************************** [first_command] ************************************** \n");
    usrmotReadZucmotStatus(&zucmotStatus);
    for (int i = 0; i < zucStatus->motion.robot_joint_num; i++)
    {
        last_tc_goal_jointpos[i] = zucmotStatus.joint_status[i].pos_cmd;
        last_tc_blend_jointpos[i] = last_tc_goal_jointpos[i];
        last_tc_blend_jointpos_sec[i] = last_tc_goal_jointpos[i];
        for (int j = 0; j < REFIT_NUM; j++) { last_tc_refit_jpos[j][i] = zucmotCommand.jog.posCmd[i]; }
        // rcs_print("first_command: joint %d last_tc_refit_jpos = %f %f %f %f %f %f\n", i, last_tc_refit_jpos[0][i], last_tc_refit_jpos[1][i],
        //               last_tc_refit_jpos[2][i], last_tc_refit_jpos[3][i], last_tc_refit_jpos[4][i], last_tc_refit_jpos[5][i]);

        cur_tc_goal_jointpos[i] = last_tc_goal_jointpos[i];  // TOPPRA_TODO : is this ok for joint move?
        cur_tc_blend_jointpos[i] = last_tc_goal_jointpos[i];
        cur_tc_blend_jointpos_sec[i] = last_tc_goal_jointpos[i];
    }
    // for(int j = 0; j < REFIT_NUM; j ++){
    //     rcs_print("last_tc_refit_num %d = %f %f %f %f %f %f\n", j, last_tc_refit_jpos[j][0], last_tc_refit_jpos[j][1], last_tc_refit_jpos[j][2],
    //     last_tc_refit_jpos[j][3], last_tc_refit_jpos[j][4], last_tc_refit_jpos[j][5]);
    // }
    last_tc_blend_r = 0.0;
    last_final_pathp = 0.0;
    lasttc_jointNum = 0;
    // rtapi_print("first_command ---> last_tc_goal_jointpos = %f %f %f %f %f %f\n",
    //             last_tc_goal_jointpos[0],
    //             last_tc_goal_jointpos[1],
    //             last_tc_goal_jointpos[2],
    //             last_tc_goal_jointpos[3],
    //             last_tc_goal_jointpos[4],
    //             last_tc_goal_jointpos[5]);
}

int movj_get_toppra_params()
{
    /* discrete tc */
    int i = 0, j = 0, discrete_num = 0, get_coefs_ret = 0;
    zucmotCommand.cur_last_blend_len = 0;  //前一运动段设置的转接半径在当前段的位置, start from 0;
    zucmotCommand.cur_blend_len = 0;       //当前段的转接半径的位置, start from 0;
    ZucJPose jpos_for_fit[MAX_POINT_NUM], blend_jpos_for_fit[MAX_BLEND_LEN];
    if (last_tc_blend_r > SMALL)
    {
        get_lasttc_blend_jpos(blend_jpos_for_fit);
    }
    if (blend_seg_jpos.jpos != NULL)
    {
        blend_seg_jpos.rear = 0;
        blend_seg_jpos.front = 0;
    }

    /* 得到离散的关节位置以及转接点所在的位置 */
    int discrete_ret = get_joint_discrete_info(discrete_num, jpos_for_fit, blend_jpos_for_fit);
    if (discrete_ret != 0)
    {  //如果只有一个点就不能再继续往下拟合了
        return discrete_ret;
    }
    get_toppra_related_params(discrete_num, jpos_for_fit, blend_jpos_for_fit);
    return discrete_ret;
}

int movl_get_toppra_params(ZucPose start_cartpos, KINEMATICS_FORWARD_FLAGS* toppra_fflags, KINEMATICS_INVERSE_FLAGS* toppra_iflags, double tmag_abc)
{
    /* discrete tc */
    int i = 0, j = 0, discrete_num = 0, get_coefs_ret = 0;
    zucmotCommand.cur_last_blend_len = 0;  //前一运动段设置的转接半径在当前段的位置,start from 0;
    zucmotCommand.cur_blend_len = 0;       //当前段的转接半径的位置, start from 0;
    ZucJPose jpos_for_fit[MAX_POINT_NUM], blend_jpos_for_fit[MAX_BLEND_LEN];

    if (last_tc_blend_r > SMALL)
    {
        get_lasttc_blend_jpos(blend_jpos_for_fit);
    }
    if (blend_seg_jpos.jpos != NULL)
    {
        blend_seg_jpos.rear = 0;
        blend_seg_jpos.front = 0;
    }

    /* 得到离散的关节位置以及转接点所在的位置 */
    int discrete_ret = 0;
    if (!zucmotCommand.trajlinearmov.using_abc_vel)
    {
        discrete_ret = get_movl_discrete_info(start_cartpos, discrete_num, jpos_for_fit, blend_jpos_for_fit, toppra_fflags, toppra_iflags);
    }
    else
    {
        discrete_ret = get_movl_discrete_info_abc(start_cartpos, discrete_num, jpos_for_fit, blend_jpos_for_fit, toppra_fflags, toppra_iflags, tmag_abc);
    }

    if (discrete_ret != 0)
    {  //如果只有一个点就不能再继续往下拟合了
        rtapi_print(" ************** [movl_get_toppra_params] ---> discrete_ret != 0 ************** \n");
        return discrete_ret;
    }
    get_toppra_related_params(discrete_num, jpos_for_fit, blend_jpos_for_fit);
    return discrete_ret;
}

int movc_get_toppra_params()
{
    /* discrete tc */
    int i = 0, j = 0, discrete_num = 0, get_coefs_ret = 0;
    zucmotCommand.cur_last_blend_len = 0;  //前一运动段设置的转接半径在当前段的位置,start from 0;
    zucmotCommand.cur_blend_len = 0;       //当前段的转接半径的位置, start from 0;
    ZucJPose jpos_for_fit[MAX_POINT_NUM], blend_jpos_for_fit[MAX_BLEND_LEN];

    if (last_tc_blend_r > SMALL)
    {
        get_lasttc_blend_jpos(blend_jpos_for_fit);
    }
    if (blend_seg_jpos.jpos != NULL)
    {
        blend_seg_jpos.rear = 0;
        blend_seg_jpos.front = 0;
    }

    /* 得到离散的关节位置以及转接点所在的位置 */
    int discrete_ret = 0;
    discrete_ret = get_movc_discrete_info(discrete_num, jpos_for_fit, blend_jpos_for_fit);

    if (discrete_ret != 0)
    {  //如果只有一个点就不能再继续往下拟合了
        rtapi_print(" ************** [movc_get_toppra_params] ---> discrete_ret != 0 ************** \n");
        return discrete_ret;
    }
    get_toppra_related_params(discrete_num, jpos_for_fit, blend_jpos_for_fit);
    return discrete_ret;
}

int get_joint_discrete_info(int& discrete_num, ZucJPose* jpos_for_fit, ZucJPose* blend_jpos_for_fit)
{
    int i = 0;
    double progress = 0.0, tar_in_jpos, tar_in_cartpos, delta_x, delta_y, delta_z;  //tar_in_cartpos for judge blend_r
    ZucPose discrete_cartpos, last_tc_goal_cartpos, cur_tc_goal_cartpos;
    KINEMATICS_FORWARD_FLAGS toppra_fflags = 0;
    KINEMATICS_INVERSE_FLAGS toppra_iflags = 0;
    /* update cur_tc_goal_jointpos */
    cur_tc_goal_jointpos[0] = zucmotCommand.trajjointmov.pos.tran.x;
    cur_tc_goal_jointpos[1] = zucmotCommand.trajjointmov.pos.tran.y;
    cur_tc_goal_jointpos[2] = zucmotCommand.trajjointmov.pos.tran.z;
    cur_tc_goal_jointpos[3] = zucmotCommand.trajjointmov.pos.a;
    cur_tc_goal_jointpos[4] = zucmotCommand.trajjointmov.pos.b;
    cur_tc_goal_jointpos[5] = zucmotCommand.trajjointmov.pos.c;

    // rtapi_print("[get_joint_discrete_info] ---> last_tc_goal_jointpos = %f %f %f %f %f %f\n", last_tc_goal_jointpos[0], last_tc_goal_jointpos[1], last_tc_goal_jointpos[2], last_tc_goal_jointpos[3],
    //                 last_tc_goal_jointpos[4], last_tc_goal_jointpos[5]);
    /* 计算笛卡尔空间长度，若不能够按照指令转接半径进行转接，则设置合理的转接半径 */
    kinematicsForward(&(zucmotConfig.tool_offset),
                      last_tc_goal_jointpos,
                      &last_tc_goal_cartpos,
                      &toppra_fflags,
                      &toppra_iflags,
                      &(zucmotConfig.base_offset),
                      &(zucmotConfig.user_offset));
    kinematicsForward(&(zucmotConfig.tool_offset),
                      cur_tc_goal_jointpos,
                      &cur_tc_goal_cartpos,
                      &toppra_fflags,
                      &toppra_iflags,
                      &(zucmotConfig.base_offset),
                      &(zucmotConfig.user_offset));
    delta_x = cur_tc_goal_cartpos.tran.x - last_tc_goal_cartpos.tran.x;
    delta_y = cur_tc_goal_cartpos.tran.y - last_tc_goal_cartpos.tran.y;
    delta_z = cur_tc_goal_cartpos.tran.z - last_tc_goal_cartpos.tran.z;
    tar_in_cartpos = pmSqrt(pmSq(delta_x) + pmSq(delta_y) + pmSq(delta_z));
    // rtapi_print(" ------------------ tar_in_cartpos = %f ------------------\n", tar_in_cartpos);
    if (cur_tc_blend_r > tar_in_cartpos * 0.45)
    {
        cur_tc_blend_r = tar_in_cartpos * 0.45;
    }
    if (last_tc_blend_r > tar_in_cartpos * 0.45)
    {
        last_tc_blend_r = tar_in_cartpos * 0.45;
    }

    /* 为后续拟合获得离散点, 并且控制离散点数不超过MAX_POINT_NUM，后续是否需要改进？ */
    int jointNum = zucmotCommand.joint;

    // rtapi_print("jointNum = %d, cur_tc_goal_jointpos[jointNum] = %f, last_tc_goal_jointpos[jointNum] = %f\n", jointNum, cur_tc_goal_jointpos[jointNum], last_tc_goal_jointpos[jointNum]);
    tar_in_jpos = fabs(cur_tc_goal_jointpos[jointNum] - last_tc_goal_jointpos[jointNum]);  //如果有笛卡尔空间运动就不能这么计算
    // rtapi_print(" ######################################## [get_joint_discrete_info] ---> tar_in_jpos = %f\n", tar_in_jpos);
    if (tar_in_jpos < SMALL)
    {  //应该不会出现这种情况
        rtapi_print("tar_in_jpos == 0.0, cur_tc_goal_jointpos[jointNum] = %f\n", cur_tc_goal_jointpos[jointNum]);
        zucmotCommand.already_inpos = 1;
        cur_tc_blend_r = 0.0;
        last_final_pathp = 0.0;
        for (int i = 0; i < zucStatus->motion.robot_joint_num; i++)
        {
            cur_tc_blend_jointpos[i] = cur_tc_goal_jointpos[i];
            cur_tc_blend_jointpos_sec[i] = cur_tc_goal_jointpos[i];
        }
        rtapi_print("[get_joint_discrete_info]  --->  already in position\n");
        return 1;
    }
    else
    {
        zucmotCommand.already_inpos = 0;
    }

    double increment = tar_in_jpos / (MAX_POINT_NUM - 1);
    if ((zucmotCommand.disjlen <= 0) || (zucmotCommand.disjlen > 1.5))
    {
        zucmotCommand.disjlen = DISCRETE_JOINT_LENGTH;
    }

    if (increment < zucmotCommand.disjlen)
    {
        increment = zucmotCommand.disjlen;
    }
    if ((tar_in_jpos / increment) < 20)
    {
        // increment = tar_in_jpos / 4;
        rtapi_print("[get_movj_discrete_info]: too short line\n");
        return 1;
    }

    int shorter_than_last_blend_r = 1, greater_than_cur_blend_r = 1, not_get_blend_secpos = 1;  // 记录转接点的位置，拟合时转接段权重非常小
    double len_from_last_goalp = 0.0, len_from_cur_goalp = 0.0;
    if (last_tc_blend_r <= SMALL)
    {
        shorter_than_last_blend_r = 0;
    }
    if (cur_tc_blend_r <= SMALL)
    {
        greater_than_cur_blend_r = 0;
        not_get_blend_secpos = 0;
        for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
        {
            cur_tc_blend_jointpos[i] = cur_tc_goal_jointpos[i];
            cur_tc_blend_jointpos_sec[i] = cur_tc_goal_jointpos[i];
        }
    }

    discrete_num = 0;
    progress = 0.0;
    BlendJpos add_blend_jpos;  //添加当前运动段转接点到目标点之间的离散关节位置
    int en_ret = -1;
    while (progress <= tar_in_jpos)
    {
        if (fabs(progress - tar_in_jpos) < SMALL)
        {
            progress = tar_in_jpos;
        }

        for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
        {
            jpos_for_fit[discrete_num].jpos[i] = last_tc_goal_jointpos[i] + progress / tar_in_jpos * (cur_tc_goal_jointpos[i] - last_tc_goal_jointpos[i]);
        }
        kinematicsForward(&(zucmotConfig.tool_offset),
                          jpos_for_fit[discrete_num].jpos,
                          &discrete_cartpos,
                          &toppra_fflags,
                          &toppra_iflags,
                          &(zucmotConfig.base_offset),
                          &(zucmotConfig.user_offset));
        if (shorter_than_last_blend_r)
        {
            delta_x = discrete_cartpos.tran.x - last_tc_goal_cartpos.tran.x;
            delta_y = discrete_cartpos.tran.y - last_tc_goal_cartpos.tran.y;
            delta_z = discrete_cartpos.tran.z - last_tc_goal_cartpos.tran.z;
            len_from_last_goalp = pmSqrt(pmSq(delta_x) + pmSq(delta_y) + pmSq(delta_z));
            if (len_from_last_goalp >= last_tc_blend_r)
            {
                zucmotCommand.cur_last_blend_len = discrete_num;
                shorter_than_last_blend_r = 0;
            }

            if (progress != 0.0)
            {
                for (int i = 0; i < zucStatus->motion.robot_joint_num; i++)
                {
                    blend_jpos_for_fit[lasttc_left_len + discrete_num - 1].jpos[i] = jpos_for_fit[discrete_num].jpos[i];
                }
            }
        }

        if (!greater_than_cur_blend_r && not_get_blend_secpos)
        {
            for (i = 0; i < zucStatus->motion.robot_joint_num; i++) { cur_tc_blend_jointpos_sec[i] = jpos_for_fit[discrete_num].jpos[i]; }
            not_get_blend_secpos = 0;
        }

        if (greater_than_cur_blend_r)
        {
            delta_x = discrete_cartpos.tran.x - cur_tc_goal_cartpos.tran.x;
            delta_y = discrete_cartpos.tran.y - cur_tc_goal_cartpos.tran.y;
            delta_z = discrete_cartpos.tran.z - cur_tc_goal_cartpos.tran.z;
            len_from_cur_goalp = pmSqrt(pmSq(delta_x) + pmSq(delta_y) + pmSq(delta_z));
            if (len_from_cur_goalp <= cur_tc_blend_r)
            {
                greater_than_cur_blend_r = 0;
                // rtapi_print("************* cur_blend_pos = %f %f %f %f %f %f\n", jpos_for_fit[discrete_num].jpos[0], jpos_for_fit[discrete_num].jpos[1], jpos_for_fit[discrete_num].jpos[2],
                //             jpos_for_fit[discrete_num].jpos[3], jpos_for_fit[discrete_num].jpos[4], jpos_for_fit[discrete_num].jpos[5]);
            }
            zucmotCommand.cur_blend_len = discrete_num;
            for (i = 0; i < zucStatus->motion.robot_joint_num; i++) { cur_tc_blend_jointpos[i] = jpos_for_fit[discrete_num].jpos[i]; }
        }
        if ((cur_tc_blend_r > SMALL) && !greater_than_cur_blend_r)
        {
            for (i = 0; i < zucStatus->motion.robot_joint_num; i++) { add_blend_jpos.blendjpos[i] = jpos_for_fit[discrete_num].jpos[i]; }
            en_ret = -1;
            en_ret = BlendEnqueue(add_blend_jpos);
            if (en_ret != 0)
            {
                cur_tc_blend_r = 0;
                zucmotCommand.cur_blend_len = 0;
            }
        }

        // if (progress == 0.0)
        // {
        zucmotCommand.path_info[0].min_time = 0.0;
        // }
        // else
        // {
        //     zucmotCommand.path_info[discrete_num].min_time =
        //         fabs(jpos_for_fit[discrete_num].jpos[zucmotCommand.joint] - jpos_for_fit[discrete_num - 1].jpos[zucmotCommand.joint]) /
        //         trajjointmov.ini_maxvel;
        // }
        discrete_num += 1;
        if (progress == tar_in_jpos)
        {
            if (cur_tc_blend_r <= SMALL)
            {
                zucmotCommand.cur_blend_len = discrete_num - 1;
            }
            break;
        }
        progress += increment;
        if (progress > tar_in_jpos)
        {
            progress = tar_in_jpos;
        }
    }

    return 0;
}

int get_unit_vector(double* vector, double mag, double* unit_vector)
{
    int i = 0;
    if (mag > CLOSE_TO_ZERO)
    {
        for (i = 0; i < 3; i++) { unit_vector[i] = vector[i] / mag; }
    }
    else
    {
        for (i = 0; i < 3; i++) { unit_vector[i] = 0.0; }
    }
    return 0;
}

int get_movl_discrete_info(ZucPose start_cartpos,
                           int& discrete_num,
                           ZucJPose* jpos_for_fit,
                           ZucJPose* blend_jpos_for_fit,
                           KINEMATICS_FORWARD_FLAGS* toppra_fflags,
                           KINEMATICS_INVERSE_FLAGS* toppra_iflags)  //blend_jpos_for_fit中存放的是转接段的离散点
{
    int i = 0;
    double progress = 0.0, last_progress = 0.0, tar_in_cartpos, delta_x, delta_y, delta_z;  //tar_in_cartpos for judge blend_r
    ZucPose discrete_cartpos;
    double dist_vector[3] = {0}, dist_unit_vector[3] = {0};

    /* 计算笛卡尔空间长度，若不能够按照指令转接半径进行转接，则设置合理的转接半径 */
    delta_x = zucmotCommand.trajlinearmov.pos.tran.x - start_cartpos.tran.x;
    delta_y = zucmotCommand.trajlinearmov.pos.tran.y - start_cartpos.tran.y;
    delta_z = zucmotCommand.trajlinearmov.pos.tran.z - start_cartpos.tran.z;

    tar_in_cartpos = pmSqrt(pmSq(delta_x) + pmSq(delta_y) + pmSq(delta_z));
    // rtapi_print("delta_x/y/z = %f, %f, %f, tar_in_cartpos = %f\n", delta_x, delta_y, delta_z, tar_in_cartpos);
    // tar_in_cartpos too small

    dist_vector[0] = delta_x;
    dist_vector[1] = delta_y;
    dist_vector[2] = delta_z;
    get_unit_vector(dist_vector, tar_in_cartpos, dist_unit_vector);
    if (cur_tc_blend_r > tar_in_cartpos * 0.45)
    {
        cur_tc_blend_r = tar_in_cartpos * 0.45;
    }
    if (last_tc_blend_r > tar_in_cartpos * 0.45)
    {
        last_tc_blend_r = tar_in_cartpos * 0.45;
    }

    /* 为后续拟合获得离散点, 并且控制离散点数不超过MAX_POINT_NUM，后续是否需要改进？ */
    if (tar_in_cartpos < SMALL)
    {  //应该不会出现这种情况
        zucmotCommand.already_inpos = 1;
        cur_tc_blend_r = 0.0;
        last_final_pathp = 0.0;
        for (int i = 0; i < zucStatus->motion.robot_joint_num; i++) { cur_tc_blend_jointpos[i] = cur_tc_goal_jointpos[i]; }
        rtapi_print("[get_movl_discrete_info]  --->  already in position\n");
        return 1;
    }
    else
    {
        zucmotCommand.already_inpos = 0;
    }

    double increment = tar_in_cartpos / (MAX_POINT_NUM - 1);
    if ((zucmotCommand.disllen <= 0) || (zucmotCommand.disllen > 5))
    {
        zucmotCommand.disllen = DISCRETE_CART_LENGTH;
    }

    if (increment < zucmotCommand.disllen)
    {
        increment = zucmotCommand.disllen;
    }
    if ((tar_in_cartpos / increment) < 20)
    {
        rtapi_print("[get_movl_discrete_info]: too short line\n");
        return 1;
        // increment = tar_in_cartpos / 4;
    }
    rtapi_print(" ----------------------------------------------------- [get_movl_discrete_info] increment = %f\n", increment);
    int shorter_than_last_blend_r = 1, greater_than_cur_blend_r = 1, not_get_blend_secpos = 1;  // 记录转接点的位置，拟合时转接段权重非常小
    double len_from_last_goalp = 0.0, len_from_cur_goalp = 0.0;
    if (last_tc_blend_r <= SMALL)
    {
        shorter_than_last_blend_r = 0;
    }
    if (cur_tc_blend_r <= SMALL)
    {
        greater_than_cur_blend_r = 0;
        not_get_blend_secpos = 0;
    }

    discrete_num = 0;
    progress = 0.0;
    last_progress = 0.0;
    BlendJpos add_blend_jpos;  //添加当前运动段转接点到目标点之间的离散关节位置
    int en_ret = -1;
    double linearvel = zucmotCommand.trajlinearmov.ini_maxvel;
    if (linearvel > tcp_vel_limit_toppra)
    {
        linearvel = tcp_vel_limit_toppra;
    }
    while (progress <= tar_in_cartpos)
    {
        if (fabs(progress - tar_in_cartpos) < SMALL)
        {
            progress = tar_in_cartpos;
        }
        get_line_position(&start_cartpos, dist_unit_vector, progress, &discrete_cartpos);
        get_line_gesture(&start_cartpos, &zucmotCommand.trajlinearmov.pos, (progress / tar_in_cartpos), &discrete_cartpos);
        if (progress == 0)
        {
            for (i = 0; i < zucStatus->motion.robot_joint_num; i++) { jpos_for_fit[discrete_num].jpos[i] = cur_tc_goal_jointpos[i]; }
            zucmotCommand.path_info[0].min_time = 0.0;
        }
        else
        {
            for (i = 0; i < zucStatus->motion.robot_joint_num; i++) { jpos_for_fit[discrete_num].jpos[i] = jpos_for_fit[discrete_num - 1].jpos[i]; }
            int inv_ret = kinematicsInverse(&zucmotConfig.tool_offset,
                                            &discrete_cartpos,
                                            jpos_for_fit[discrete_num].jpos,
                                            toppra_iflags,
                                            toppra_fflags,
                                            &zucmotConfig.base_offset,
                                            &zucmotConfig.user_offset);
            if (inv_ret != 0)
            {
                rtapi_print(" ************************************ inv_ret != 0 ************************************ \n");
                return 1;
            }
            // zucmotCommand.path_info[discrete_num].min_time = fabs(progress - last_progress) / linearvel;
        }
        // rtapi_print("[get_movl_discrete_info] ---> discrete_cartpos =  %f %f %f %f %f %f\n", discrete_cartpos.tran.x, discrete_cartpos.tran.y, discrete_cartpos.tran.z, discrete_cartpos.a, discrete_cartpos.b, discrete_cartpos.c);
        // rtapi_print(" **************************************** %dth %f %f %f %f %f %f\n", discrete_num, jpos_for_fit[discrete_num].jpos[0], jpos_for_fit[discrete_num].jpos[1], jpos_for_fit[discrete_num].jpos[2],
        //         jpos_for_fit[discrete_num].jpos[3], jpos_for_fit[discrete_num].jpos[4], jpos_for_fit[discrete_num].jpos[5]);

        if (shorter_than_last_blend_r)
        {
            if (progress >= last_tc_blend_r)
            {
                zucmotCommand.cur_last_blend_len = discrete_num;
                shorter_than_last_blend_r = 0;
            }
            if (progress != 0.0)
            {
                for (int i = 0; i < zucStatus->motion.robot_joint_num; i++)
                {
                    blend_jpos_for_fit[lasttc_left_len + discrete_num - 1].jpos[i] = jpos_for_fit[discrete_num].jpos[i];
                }
            }
            // rtapi_print("shorter than last blend r: %f %f %f %f %f %f\n", jpos_for_fit[discrete_num].jpos[0], jpos_for_fit[discrete_num].jpos[1], jpos_for_fit[discrete_num].jpos[2],
            //             jpos_for_fit[discrete_num].jpos[3], jpos_for_fit[discrete_num].jpos[4], jpos_for_fit[discrete_num].jpos[5]);
        }

        if (!greater_than_cur_blend_r && not_get_blend_secpos)
        {
            for (i = 0; i < zucStatus->motion.robot_joint_num; i++) { cur_tc_blend_jointpos_sec[i] = jpos_for_fit[discrete_num].jpos[i]; }
            not_get_blend_secpos = 0;
        }

        if (greater_than_cur_blend_r)
        {
            if ((tar_in_cartpos - progress) <= cur_tc_blend_r)
            {
                greater_than_cur_blend_r = 0;
            }
            zucmotCommand.cur_blend_len = discrete_num;
            for (i = 0; i < zucStatus->motion.robot_joint_num; i++) { cur_tc_blend_jointpos[i] = jpos_for_fit[discrete_num].jpos[i]; }
        }
        // if(zucmotCommand.cur_blend_len != discrete_num){
        //     rtapi_print("*********************************************************** zucmotCommand.cur_blend_len = %d, cur_tc_blend_jointpos = %f %f %f %f %f %f\n", zucmotCommand.cur_blend_len,
        //             cur_tc_blend_jointpos[0], cur_tc_blend_jointpos[1], cur_tc_blend_jointpos[2], cur_tc_blend_jointpos[3], cur_tc_blend_jointpos[4], cur_tc_blend_jointpos[5]);
        // }
        // if(!shorter_than_last_blend_r && !greater_than_cur_blend_r && cur_tc_blend_r > SMALL){
        //     rtapi_print("greater_than_cur_blend_r: %f %f %f %f %f %f\n", jpos_for_fit[discrete_num].jpos[0], jpos_for_fit[discrete_num].jpos[1], jpos_for_fit[discrete_num].jpos[2],
        //                 jpos_for_fit[discrete_num].jpos[3], jpos_for_fit[discrete_num].jpos[4], jpos_for_fit[discrete_num].jpos[5]);
        // }
        if ((cur_tc_blend_r > SMALL) && !greater_than_cur_blend_r)
        {
            for (i = 0; i < zucStatus->motion.robot_joint_num; i++) { add_blend_jpos.blendjpos[i] = jpos_for_fit[discrete_num].jpos[i]; }
            en_ret = -1;
            en_ret = BlendEnqueue(add_blend_jpos);
            if (en_ret != 0)
            {
                cur_tc_blend_r = 0;
                zucmotCommand.cur_blend_len = 0;
            }
        }

        discrete_num += 1;
        if (progress == tar_in_cartpos)
        {
            if (cur_tc_blend_r <= SMALL)
            {
                zucmotCommand.cur_blend_len = discrete_num - 1;
            }
            break;
        }
        last_progress = progress;
        progress += increment;
        if (progress > tar_in_cartpos)
        {
            progress = tar_in_cartpos;
        }
    }

    /* update cur_tc_goal_jointpos */
    if (discrete_num > 0)
    {
        cur_tc_goal_jointpos[0] = jpos_for_fit[discrete_num - 1].jpos[0];
        cur_tc_goal_jointpos[1] = jpos_for_fit[discrete_num - 1].jpos[1];
        cur_tc_goal_jointpos[2] = jpos_for_fit[discrete_num - 1].jpos[2];
        cur_tc_goal_jointpos[3] = jpos_for_fit[discrete_num - 1].jpos[3];
        cur_tc_goal_jointpos[4] = jpos_for_fit[discrete_num - 1].jpos[4];
        cur_tc_goal_jointpos[5] = jpos_for_fit[discrete_num - 1].jpos[5];
    }

    if (cur_tc_blend_r <= SMALL)
    {
        for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
        {
            cur_tc_blend_jointpos[i] = cur_tc_goal_jointpos[i];
            cur_tc_blend_jointpos_sec[i] = cur_tc_goal_jointpos[i];
        }
    }
    return 0;
}

int get_movl_discrete_info_abc(ZucPose start_cartpos,
                               int& discrete_num,
                               ZucJPose* jpos_for_fit,
                               ZucJPose* blend_jpos_for_fit,
                               KINEMATICS_FORWARD_FLAGS* toppra_fflags,
                               KINEMATICS_INVERSE_FLAGS* toppra_iflags,
                               double tmag_abc)  //blend_jpos_for_fit中存放的是转接段的离散点
{
    int i = 0;
    double progress = 0.0, last_progress = 0.0, tar_in_cartpos = 0.0, tar_in_cart_abc = 0.0, delta_x = 0.0, delta_y = 0.0,
           delta_z = 0.0;  //tar_in_cartpos for judge blend_r
    ZucPose discrete_cartpos;
    double dist_vector[3] = {0}, dist_unit_vector[3] = {0};

    /* 计算笛卡尔空间长度，若不能够按照指令转接半径进行转接，则设置合理的转接半径 */
    delta_x = zucmotCommand.trajlinearmov.pos.tran.x - start_cartpos.tran.x;
    delta_y = zucmotCommand.trajlinearmov.pos.tran.y - start_cartpos.tran.y;
    delta_z = zucmotCommand.trajlinearmov.pos.tran.z - start_cartpos.tran.z;

    tar_in_cartpos = pmSqrt(pmSq(delta_x) + pmSq(delta_y) + pmSq(delta_z));
    // rtapi_print("delta_x/y/z = %f, %f, %f, tar_in_cartpos = %f\n", delta_x, delta_y, delta_z, tar_in_cartpos);
    // tar_in_cartpos too small

    dist_vector[0] = delta_x;
    dist_vector[1] = delta_y;
    dist_vector[2] = delta_z;
    get_unit_vector(dist_vector, tar_in_cartpos, dist_unit_vector);
    // rtapi_print("[get_movl_discrete_info_abc] ---> start_cartpos = %f %f %f %f %f %f, end_pos = %f %f %f %f %f %f, delta = %f %f %f, dist_unit_vector = %f %f %f\n",
    //                     start_cartpos.tran.x, start_cartpos.tran.y, start_cartpos.tran.z, start_cartpos.a,
    //                     start_cartpos.b, start_cartpos.c, zucmotCommand.trajlinearmov.pos.tran.x, zucmotCommand.trajlinearmov.pos.tran.y, zucmotCommand.trajlinearmov.pos.tran.z,
    //                     zucmotCommand.trajlinearmov.pos.a, zucmotCommand.trajlinearmov.pos.b, zucmotCommand.trajlinearmov.pos.c,
    //                     dist_vector[0], dist_vector[1], dist_vector[2], dist_unit_vector[0], dist_unit_vector[1], dist_unit_vector[2]);

    if (cur_tc_blend_r > tar_in_cartpos * 0.45)
    {
        cur_tc_blend_r = tar_in_cartpos * 0.45;
    }
    if (last_tc_blend_r > tar_in_cartpos * 0.45)
    {
        last_tc_blend_r = tar_in_cartpos * 0.45;
    }

    /* 为后续拟合获得离散点, 并且控制离散点数不超过MAX_POINT_NUM，后续是否需要改进？ */
    if (tar_in_cartpos < SMALL && tmag_abc < SMALL)
    {  //应该不会出现这种情况
        zucmotCommand.already_inpos = 1;
        cur_tc_blend_r = 0.0;
        last_final_pathp = 0.0;
        for (int i = 0; i < zucStatus->motion.robot_joint_num; i++) { cur_tc_blend_jointpos[i] = cur_tc_goal_jointpos[i]; }
        rtapi_print("[get_movl_discrete_info_abc]  --->  already in position\n");
        return 1;
    }
    else
    {
        zucmotCommand.already_inpos = 0;
    }

    double increment = tmag_abc / (MAX_POINT_NUM - 1);
    if ((zucmotCommand.disjlen <= 0) || (zucmotCommand.disjlen > 1.5))
    {
        zucmotCommand.disjlen = DISCRETE_CART_LENGTH;
    }

    if (increment < zucmotCommand.disjlen)
    {
        increment = zucmotCommand.disjlen;
    }
    if ((tmag_abc / increment) < 20)
    {
        rtapi_print("[get_movl_discrete_info_abc]: too short line\n");
        return 1;
        // increment = tar_in_cartpos / 4;
    }
    rtapi_print(" ----------------------------------------------------- [get_movl_discrete_info_abc] increment = %f\n", increment);
    int shorter_than_last_blend_r = 1, greater_than_cur_blend_r = 1, not_get_blend_secpos = 1;  // 记录转接点的位置，拟合时转接段权重非常小
    double len_from_last_goalp = 0.0, len_from_cur_goalp = 0.0;
    if (last_tc_blend_r <= SMALL)
    {
        shorter_than_last_blend_r = 0;
    }
    if (cur_tc_blend_r <= SMALL)
    {
        greater_than_cur_blend_r = 0;
        not_get_blend_secpos = 0;
    }

    discrete_num = 0;
    progress = 0.0;
    last_progress = 0.0;
    BlendJpos add_blend_jpos;  //添加当前运动段转接点到目标点之间的离散关节位置
    int en_ret = -1;
    double dist_from_start = 0.0;
    while (progress <= tmag_abc)
    {
        if (fabs(progress - tmag_abc) < SMALL)
        {
            progress = tmag_abc;
        }
        dist_from_start = progress / tmag_abc * tar_in_cartpos;
        get_line_position(&start_cartpos, dist_unit_vector, dist_from_start, &discrete_cartpos);
        get_line_gesture(&start_cartpos, &zucmotCommand.trajlinearmov.pos, (progress / tmag_abc), &discrete_cartpos);
        if (progress == 0)
        {
            for (i = 0; i < zucStatus->motion.robot_joint_num; i++) { jpos_for_fit[discrete_num].jpos[i] = cur_tc_goal_jointpos[i]; }
            zucmotCommand.path_info[0].min_time = 0.0;
        }
        else
        {
            for (i = 0; i < zucStatus->motion.robot_joint_num; i++) { jpos_for_fit[discrete_num].jpos[i] = jpos_for_fit[discrete_num - 1].jpos[i]; }
            int inv_ret = kinematicsInverse(&zucmotConfig.tool_offset,
                                            &discrete_cartpos,
                                            jpos_for_fit[discrete_num].jpos,
                                            toppra_iflags,
                                            toppra_fflags,
                                            &zucmotConfig.base_offset,
                                            &zucmotConfig.user_offset);
            if (inv_ret != 0)
            {
                rtapi_print(" ************************************ inv_ret != 0 ************************************ \n");
                return 1;
            }
            zucmotCommand.path_info[discrete_num].min_time =
                fabs(progress - last_progress) / zucmotCommand.trajlinearmov.ori_vel;  // attention here, the velocity
            // rtapi_print(" --- --- --- --- %dth min_time = %f \n", discrete_num, zucmotCommand.path_info[discrete_num].min_time);
        }
        // rtapi_print("[get_movl_discrete_info] ---> discrete_cartpos =  %f %f %f %f %f %f\n", discrete_cartpos.tran.x, discrete_cartpos.tran.y, discrete_cartpos.tran.z, discrete_cartpos.a, discrete_cartpos.b, discrete_cartpos.c);
        // rtapi_print(" **************************************** %dth %f %f %f %f %f %f\n", discrete_num, jpos_for_fit[discrete_num].jpos[0], jpos_for_fit[discrete_num].jpos[1], jpos_for_fit[discrete_num].jpos[2],
        //         jpos_for_fit[discrete_num].jpos[3], jpos_for_fit[discrete_num].jpos[4], jpos_for_fit[discrete_num].jpos[5]);
        // delta_x = discrete_cartpos.tran.x - start_cartpos.tran.x;
        // delta_y = discrete_cartpos.tran.y - start_cartpos.tran.y;
        // delta_z = discrete_cartpos.tran.z - start_cartpos.tran.z;
        // dist_from_start = pmSqrt(pmSq(delta_x) + pmSq(delta_y) + pmSq(delta_z));

        if (shorter_than_last_blend_r)
        {
            if (dist_from_start >= last_tc_blend_r)
            {
                zucmotCommand.cur_last_blend_len = discrete_num;
                shorter_than_last_blend_r = 0;
            }
            if (dist_from_start != 0.0)
            {
                for (int i = 0; i < zucStatus->motion.robot_joint_num; i++)
                {
                    blend_jpos_for_fit[lasttc_left_len + discrete_num - 1].jpos[i] = jpos_for_fit[discrete_num].jpos[i];
                }  //TODO
            }
            // rtapi_print("shorter than last blend r: %f %f %f %f %f %f\n", jpos_for_fit[discrete_num].jpos[0], jpos_for_fit[discrete_num].jpos[1], jpos_for_fit[discrete_num].jpos[2],
            //             jpos_for_fit[discrete_num].jpos[3], jpos_for_fit[discrete_num].jpos[4], jpos_for_fit[discrete_num].jpos[5]);
        }

        if (!greater_than_cur_blend_r && not_get_blend_secpos)
        {
            for (i = 0; i < zucStatus->motion.robot_joint_num; i++) { cur_tc_blend_jointpos_sec[i] = jpos_for_fit[discrete_num].jpos[i]; }
            not_get_blend_secpos = 0;
        }

        if (greater_than_cur_blend_r)
        {
            if ((tar_in_cartpos - dist_from_start) <= cur_tc_blend_r)
            {
                greater_than_cur_blend_r = 0;
            }
            zucmotCommand.cur_blend_len = discrete_num;
            for (i = 0; i < zucStatus->motion.robot_joint_num; i++) { cur_tc_blend_jointpos[i] = jpos_for_fit[discrete_num].jpos[i]; }
        }
        // if(zucmotCommand.cur_blend_len != discrete_num){
        //     rtapi_print("*********************************************************** zucmotCommand.cur_blend_len = %d, cur_tc_blend_jointpos = %f %f %f %f %f %f\n", zucmotCommand.cur_blend_len,
        //             cur_tc_blend_jointpos[0], cur_tc_blend_jointpos[1], cur_tc_blend_jointpos[2], cur_tc_blend_jointpos[3], cur_tc_blend_jointpos[4], cur_tc_blend_jointpos[5]);
        // }
        // if(!shorter_than_last_blend_r && !greater_than_cur_blend_r && cur_tc_blend_r > SMALL){
        //     rtapi_print("greater_than_cur_blend_r: %f %f %f %f %f %f\n", jpos_for_fit[discrete_num].jpos[0], jpos_for_fit[discrete_num].jpos[1], jpos_for_fit[discrete_num].jpos[2],
        //                 jpos_for_fit[discrete_num].jpos[3], jpos_for_fit[discrete_num].jpos[4], jpos_for_fit[discrete_num].jpos[5]);
        // }
        if ((cur_tc_blend_r > SMALL) && !greater_than_cur_blend_r)
        {
            for (i = 0; i < zucStatus->motion.robot_joint_num; i++) { add_blend_jpos.blendjpos[i] = jpos_for_fit[discrete_num].jpos[i]; }  //*******TODO
            en_ret = -1;
            en_ret = BlendEnqueue(add_blend_jpos);
            if (en_ret != 0)
            {
                cur_tc_blend_r = 0;
                zucmotCommand.cur_blend_len = 0;
            }
        }

        discrete_num += 1;
        if (progress == tmag_abc)
        {
            if (cur_tc_blend_r <= SMALL)
            {
                zucmotCommand.cur_blend_len = discrete_num - 1;
            }
            break;
        }
        last_progress = progress;
        progress += increment;
        if (progress > tmag_abc)
        {
            progress = tmag_abc;
        }
    }

    /* update cur_tc_goal_jointpos */
    if (discrete_num > 0)
    {
        cur_tc_goal_jointpos[0] = jpos_for_fit[discrete_num - 1].jpos[0];
        cur_tc_goal_jointpos[1] = jpos_for_fit[discrete_num - 1].jpos[1];
        cur_tc_goal_jointpos[2] = jpos_for_fit[discrete_num - 1].jpos[2];
        cur_tc_goal_jointpos[3] = jpos_for_fit[discrete_num - 1].jpos[3];
        cur_tc_goal_jointpos[4] = jpos_for_fit[discrete_num - 1].jpos[4];
        cur_tc_goal_jointpos[5] = jpos_for_fit[discrete_num - 1].jpos[5];
    }

    if (cur_tc_blend_r <= SMALL)
    {
        for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
        {
            cur_tc_blend_jointpos[i] = cur_tc_goal_jointpos[i];
            cur_tc_blend_jointpos_sec[i] = cur_tc_goal_jointpos[i];
        }
    }
    return 0;
}

int get_movc_discrete_info(int& discrete_num, ZucJPose* jpos_for_fit, ZucJPose* blend_jpos_for_fit)  //blend_jpos_for_fit中存放的是转接段的离散点
{
    int i = 0;
    double progress = 0.0, last_progress = 0.0, tar_in_cartpos_arc, delta_x, delta_y, delta_z;  // tar_in_cartpos_arc for judge blend_r
    ZucPose start_cartpos, discrete_cartpos;
    KINEMATICS_FORWARD_FLAGS toppra_fflags = 0;
    KINEMATICS_INVERSE_FLAGS toppra_iflags = 0;
    //首先进行一次运动学正解，获得运动段起始点的笛卡尔坐标并更新逆解标志位
    kinematicsForward(&(zucmotConfig.tool_offset),
                      cur_tc_goal_jointpos,
                      &start_cartpos,
                      &toppra_fflags,
                      &toppra_iflags,
                      &(zucmotConfig.base_offset),
                      &(zucmotConfig.user_offset));
    // rtapi_print("[get_movc_discrete_info] ---> cur_tc_goal_jointpos = %f %f %f %f %f %f\n", cur_tc_goal_jointpos[0], cur_tc_goal_jointpos[1], cur_tc_goal_jointpos[2], cur_tc_goal_jointpos[3],
    //                     cur_tc_goal_jointpos[4], cur_tc_goal_jointpos[5]);
    // rtapi_print("[get_movc_discrete_info] ---> start_cartpos = %f %f %f %f %f %f\n", start_cartpos.tran.x, start_cartpos.tran.y, start_cartpos.tran.z, start_cartpos.a,
    //                     start_cartpos.b, start_cartpos.c);

    // 确定圆心
    double sm[3] = {0}, me[3] = {0}, se[3] = {0}, sc[3] = {0};  // sc means startp-centerp
    double ms_norm = 0.0, me_norm = 0.0, se_norm = 0.0;

    sm[0] = zucmotCommand.trajmovc.movcMid.tran.x - start_cartpos.tran.x;
    sm[1] = zucmotCommand.trajmovc.movcMid.tran.y - start_cartpos.tran.y;
    sm[2] = zucmotCommand.trajmovc.movcMid.tran.z - start_cartpos.tran.z;
    me[0] = zucmotCommand.trajmovc.movcEnd.tran.x - zucmotCommand.trajmovc.movcMid.tran.x;
    me[1] = zucmotCommand.trajmovc.movcEnd.tran.y - zucmotCommand.trajmovc.movcMid.tran.y;
    me[2] = zucmotCommand.trajmovc.movcEnd.tran.z - zucmotCommand.trajmovc.movcMid.tran.z;
    se[0] = zucmotCommand.trajmovc.movcEnd.tran.x - start_cartpos.tran.x;
    se[1] = zucmotCommand.trajmovc.movcEnd.tran.y - start_cartpos.tran.y;
    se[2] = zucmotCommand.trajmovc.movcEnd.tran.z - start_cartpos.tran.z;
    ms_norm = sqrt(sm[0] * sm[0] + sm[1] * sm[1] + sm[2] * sm[2]);
    me_norm = sqrt(me[0] * me[0] + me[1] * me[1] + me[2] * me[2]);
    se_norm = sqrt(se[0] * se[0] + se[1] * se[1] + se[2] * se[2]);

    if (((ms_norm < 1e-3) || (me_norm < 1e-3)) || (se_norm < 1e-3))
    {
        //目前要求示教三点不能共线,后续若需要的话可以进行优化
        return -1;
    }
    zucmotCommand.already_inpos = 0;
    MovcStruct circle;  // include params needed for planning a movc
    int get_movcp_ret = get_movc_planning_params(&circle, &start_cartpos, &(zucmotCommand.trajmovc.movcMid), &(zucmotCommand.trajmovc.movcEnd));
    if (get_movcp_ret != 0)
    {
        return -1;
    }
    tar_in_cartpos_arc = circle.angle * circle.radius;  // length of arc
    if (cur_tc_blend_r > tar_in_cartpos_arc * 0.45)
    {
        rtapi_print("[MOVC] cur_tc_blend_r is unsuitable, reset cur_tc_blend_r according to target length\n");
        cur_tc_blend_r = tar_in_cartpos_arc * 0.45;
    }
    if (last_tc_blend_r > tar_in_cartpos_arc * 0.45)
    {
        rtapi_print("[MOVC] last_tc_blend_r is unsuitable, reset last_tc_blend_r according to target length\n");
        last_tc_blend_r = tar_in_cartpos_arc * 0.45;
    }
    double increment = tar_in_cartpos_arc / (MAX_POINT_NUM - 1);
    if ((zucmotCommand.disclen <= 0) || (zucmotCommand.disclen > 5))
    {
        zucmotCommand.disclen = DISCRETE_CART_LENGTH;
    }
    if (increment < zucmotCommand.disclen)
    {
        increment = zucmotCommand.disclen;
    }
    if ((tar_in_cartpos_arc / increment) < 20)  // TOPPRA_TODO: 限制最小的离散个数，不应该是4
    {
        rtapi_print("[get_movl_discrete_info]: too short line\n");
        return 1;
        // increment = tar_in_cartpos / 4;
    }

    int shorter_than_last_blend_r = 1, greater_than_cur_blend_r = 1, not_get_blend_secpos = 1;  // 记录转接点的位置，拟合时转接段权重非常小
    double len_from_last_goalp = 0.0, len_from_cur_goalp = 0.0;
    if (last_tc_blend_r <= SMALL)
    {
        shorter_than_last_blend_r = 0;
    }
    if (cur_tc_blend_r <= SMALL)
    {
        greater_than_cur_blend_r = 0;
        not_get_blend_secpos = 0;
    }

    discrete_num = 0;
    progress = 0.0;
    last_progress = 0.0;
    BlendJpos add_blend_jpos;  //添加当前运动段转接点到目标点之间的离散关节位置
    int en_ret = -1;
    // TOPPRA_TODO: 以下过程和获取movl离散点的过程一致，整合一下
    while (progress <= tar_in_cartpos_arc)
    {
        if (fabs(progress - tar_in_cartpos_arc) < SMALL)
        {
            progress = tar_in_cartpos_arc;
        }
        //获取离散点的位姿
        get_movc_pos(&circle, (progress / tar_in_cartpos_arc), &discrete_cartpos);
        if (progress == 0)
        {
            for (i = 0; i < zucStatus->motion.robot_joint_num; i++) { jpos_for_fit[discrete_num].jpos[i] = cur_tc_goal_jointpos[i]; }
            zucmotCommand.path_info[0].min_time = 0.0;
        }
        else
        {
            for (i = 0; i < zucStatus->motion.robot_joint_num; i++) { jpos_for_fit[discrete_num].jpos[i] = jpos_for_fit[discrete_num - 1].jpos[i]; }
            int inv_ret = kinematicsInverse(&zucmotConfig.tool_offset,
                                            &discrete_cartpos,
                                            jpos_for_fit[discrete_num].jpos,
                                            &toppra_iflags,
                                            &toppra_fflags,
                                            &zucmotConfig.base_offset,
                                            &zucmotConfig.user_offset);
            if (inv_ret != 0)
            {
                rtapi_print(" ************************************ inv_ret != 0 ************************************ \n");
                return 1;
            }
            // zucmotCommand.path_info[discrete_num].min_time = fabs(progress - last_progress) / trajmovc.ini_maxvel;
            // if (fabs(progress - tar_in_cartpos_arc) < 1e-6)
            // {
            //     rtapi_print(" the last one jposition is : %f %f %f %f %f %f, discrete_cartpos = %f %f %f %f %f %f\n", jpos_for_fit[discrete_num].jpos[0],
            //            jpos_for_fit[discrete_num].jpos[1], jpos_for_fit[discrete_num].jpos[2], jpos_for_fit[discrete_num].jpos[3],
            //            jpos_for_fit[discrete_num].jpos[4], jpos_for_fit[discrete_num].jpos[5], discrete_cartpos.tran.x, discrete_cartpos.tran.y,
            //            discrete_cartpos.tran.z, discrete_cartpos.a, discrete_cartpos.b, discrete_cartpos.c);
            // }
        }
        // rtapi_print("[get_movl_discrete_info] ---> discrete_cartpos =  %f %f %f %f %f %f\n", discrete_cartpos.tran.x, discrete_cartpos.tran.y,
        // discrete_cartpos.tran.z, discrete_cartpos.a, discrete_cartpos.b, discrete_cartpos.c);
        // rtapi_print(" **************************************** %dth %f
        // %f %f %f %f %f\n", discrete_num, jpos_for_fit[discrete_num].jpos[0], jpos_for_fit[discrete_num].jpos[1], jpos_for_fit[discrete_num].jpos[2],
        //         jpos_for_fit[discrete_num].jpos[3], jpos_for_fit[discrete_num].jpos[4], jpos_for_fit[discrete_num].jpos[5]);

        if (shorter_than_last_blend_r)
        {
            if (progress >= last_tc_blend_r)
            {
                zucmotCommand.cur_last_blend_len = discrete_num;
                shorter_than_last_blend_r = 0;
            }
            if (progress != 0.0)
            {
                for (int i = 0; i < zucStatus->motion.robot_joint_num; i++)
                {
                    blend_jpos_for_fit[lasttc_left_len + discrete_num - 1].jpos[i] = jpos_for_fit[discrete_num].jpos[i];
                }
            }

            // rtapi_print("shorter than last blend r: %f %f %f %f %f %f\n", jpos_for_fit[discrete_num].jpos[0], jpos_for_fit[discrete_num].jpos[1],
            // jpos_for_fit[discrete_num].jpos[2],
            //             jpos_for_fit[discrete_num].jpos[3], jpos_for_fit[discrete_num].jpos[4], jpos_for_fit[discrete_num].jpos[5]);
        }

        if (!greater_than_cur_blend_r && not_get_blend_secpos)
        {
            for (i = 0; i < zucStatus->motion.robot_joint_num; i++) { cur_tc_blend_jointpos_sec[i] = jpos_for_fit[discrete_num].jpos[i]; }
            not_get_blend_secpos = 0;
        }

        if (greater_than_cur_blend_r)
        {
            if ((tar_in_cartpos_arc - progress) <= cur_tc_blend_r)
            {
                greater_than_cur_blend_r = 0;
            }
            zucmotCommand.cur_blend_len = discrete_num;
            for (i = 0; i < zucStatus->motion.robot_joint_num; i++) { cur_tc_blend_jointpos[i] = jpos_for_fit[discrete_num].jpos[i]; }
        }
        // if(zucmotCommand.cur_blend_len != discrete_num){
        //     rtapi_print("*********************************************************** zucmotCommand.cur_blend_len = %d, cur_tc_blend_jointpos = %f %f %f %f %f
        //     %f\n", zucmotCommand.cur_blend_len,
        //             cur_tc_blend_jointpos[0], cur_tc_blend_jointpos[1], cur_tc_blend_jointpos[2], cur_tc_blend_jointpos[3], cur_tc_blend_jointpos[4],
        //             cur_tc_blend_jointpos[5]);
        // }
        // if(!shorter_than_last_blend_r && !greater_than_cur_blend_r && cur_tc_blend_r > SMALL){
        //     rtapi_print("greater_than_cur_blend_r: %f %f %f %f %f %f\n", jpos_for_fit[discrete_num].jpos[0], jpos_for_fit[discrete_num].jpos[1],
        //     jpos_for_fit[discrete_num].jpos[2],
        //                 jpos_for_fit[discrete_num].jpos[3], jpos_for_fit[discrete_num].jpos[4], jpos_for_fit[discrete_num].jpos[5]);
        // }
        if ((cur_tc_blend_r > SMALL) && !greater_than_cur_blend_r)
        {
            for (i = 0; i < zucStatus->motion.robot_joint_num; i++) { add_blend_jpos.blendjpos[i] = jpos_for_fit[discrete_num].jpos[i]; }
            en_ret = -1;
            en_ret = BlendEnqueue(add_blend_jpos);
            if (en_ret != 0)
            {
                cur_tc_blend_r = 0;
                zucmotCommand.cur_blend_len = 0;
            }
        }

        discrete_num += 1;
        if (progress == tar_in_cartpos_arc)
        {
            if (cur_tc_blend_r <= SMALL)
            {
                zucmotCommand.cur_blend_len = discrete_num - 1;
            }
            break;
        }
        last_progress = progress;
        progress += increment;
        if (progress > tar_in_cartpos_arc)
        {
            progress = tar_in_cartpos_arc;
        }
    }
    /* update cur_tc_goal_jointpos */
    if (discrete_num > 0)
    {
        cur_tc_goal_jointpos[0] = jpos_for_fit[discrete_num - 1].jpos[0];
        cur_tc_goal_jointpos[1] = jpos_for_fit[discrete_num - 1].jpos[1];
        cur_tc_goal_jointpos[2] = jpos_for_fit[discrete_num - 1].jpos[2];
        cur_tc_goal_jointpos[3] = jpos_for_fit[discrete_num - 1].jpos[3];
        cur_tc_goal_jointpos[4] = jpos_for_fit[discrete_num - 1].jpos[4];
        cur_tc_goal_jointpos[5] = jpos_for_fit[discrete_num - 1].jpos[5];
    }

    if (cur_tc_blend_r <= SMALL)
    {
        for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
        {
            cur_tc_blend_jointpos[i] = cur_tc_goal_jointpos[i];
            cur_tc_blend_jointpos_sec[i] = cur_tc_goal_jointpos[i];
        }
    }

    return 0;
}

PmCartesian get_movc_center(PmCartesian A, PmCartesian B, PmCartesian C)
{
    PmCartesian u, v;    //求出的新坐标系X,Y方向上的向量
    PmCartesian center;  //圆心
    PmCartesian AB, AC;  // AB: 向量AB， AC：向量AC
    PmCartesian ex, ey;  //ex: 新坐标系X轴单位向量 ，ey: 新坐标系Y轴单位向量
    PmCartesian vy, vx;  // C点做垂先和AB相交，所得的X轴上的向量。
    double xB, xC, yC;   // AB的长度。

    PmCartesian tempEx, tempEy;

    pmCartCartSub(&B, &A, &u);     //计算 u = B - A;
    pmCartCartSub(&C, &A, &v);     //计算 v = C - A;
    pmCartMag(&u, &xB);            //计算 xB = norm(u)
    pmCartUnit(&u, &ex);           //计算 ex = u/xB
    pmCartCartDot(&v, &ex, &xC);   //vx = （v * ex）*ex;
    pmCartScalMult(&ex, xC, &vx);  //计算 vy = v - vx;
    pmCartCartSub(&v, &vx, &vy);   //计算Y轴单位向量 ey = ey/|ey|
    pmCartUnit(&vy, &ey);          //计算 yC = |vy|
    pmCartMag(&vy, &yC);
    pmCartScalMult(&ex, 0.5 * xB, &tempEx);
    pmCartCartAdd(&A, &tempEx, &tempEx);
    pmCartScalMult(
        &ey, (-1 * xB * (xB - xC) * xC + xB * yC * yC) / (2 * xB * yC), &tempEy);  //计算center = A + 1/2 xB*ex + (-xB (xB - xC) xC + xB yC^2)/(2 xB yC)*ey
    pmCartCartAdd(&tempEx, &tempEy, &center);

    return center;
}

int get_movc_planning_params(MovcStruct* circle, ZucPose const* const start, ZucPose const* const mid, ZucPose const* const end)
{
    //the same as pmMovcInit()
    //计算圆心
    PmCartesian startPosition;
    PmCartesian midPosition;
    PmCartesian endPosition;

    PmCartesian center;
    PmCartesian uCenterStart;
    PmCartesian uCenterMid;
    PmCartesian uCenterEnd;
    PmCartesian normal, normal1, normal2;

    double radius;
    double theta, scm, mce, sce;
    double thetaMD;
    double startEndDot;
    double startMidDot;
    double midEndDot;
    PmRotationMatrix rotStart;
    PmRotationMatrix rotEnd;
    PmRotationMatrix rot;
    PmRotationMatrix startInv;
    PmRpy rpyStart, rpyEnd;
    PmRotationVector abc;
    double normalMag;
    double cosTheta;
    int turn;
    int use_sm = 1;
    startPosition = start->tran;
    midPosition = mid->tran;
    endPosition = end->tran;

    // rtapi_print(" --------pmMovcInit------- : startPosition =%f   %f  %f  \n  ",startPosition.x,startPosition.y,startPosition.z);
    // rtapi_print(" --------pmMovcInit------- : midPosition =%f   %f  %f \n ",midPosition.x,midPosition.y,midPosition.z);
    // rtapi_print(" --------pmMovcInit------- : end =%f   %f  %f  \n ",endPosition.x,endPosition.y,endPosition.z);

    center = get_movc_center(startPosition, midPosition, endPosition);
    // rtapi_print(" --------pmMovcInit------- : center =%f   %f  %f  \n  ",center.x,center.y,center.z);

    pmCartCartSub(&startPosition, &center, &uCenterStart);
    pmCartCartSub(&midPosition, &center, &uCenterMid);
    pmCartCartSub(&endPosition, &center, &uCenterEnd);

    // rtapi_print("uCenterStart = %f %f %f \n ",uCenterStart.x,uCenterStart.y,uCenterStart.z);
    // rtapi_print("uCenterMid = %f %f %f \n ",uCenterMid.x,uCenterMid.y,uCenterMid.z);
    // rtapi_print("uCenterMid = %f %f %f \n ",uCenterEnd.x,uCenterEnd.y,uCenterEnd.z);

    //计算半径
    pmCartMag(&uCenterStart, &radius);
    // rtapi_print(" --------pmMovcInit------- : radius =%f   \n  ",radius);

    //计算旋转方向
    //计算圆心角
    pmCartCartDot(&uCenterStart, &uCenterEnd, &startEndDot);
    cosTheta = startEndDot / radius / radius;
    if (cosTheta > 1.0)
        cosTheta = 1.0;
    else if (cosTheta < -1.0)
        cosTheta = -1.0;
    theta = acos(cosTheta);

    // rtapi_print(" --------pmMovcInit------- : theta =%f   \n  ",theta);

    //计算姿态变化，
    if (fabs(theta) < TP_MIN_ARC_ANGLE)
    {
        return TP_ERR_FAIL;
    }
    //计算法向量
    pmCartCartDot(&uCenterStart, &uCenterMid, &startMidDot);
    scm = acos(startMidDot / radius / radius);

    pmCartCartDot(&uCenterMid, &uCenterEnd, &midEndDot);
    mce = acos(midEndDot / radius / radius);

    pmCartCartDot(&uCenterStart, &uCenterEnd, &startEndDot);
    sce = acos(startEndDot / radius / radius);

    // rtapi_print(" --------pmMovcInit------- : scm =%f   mce =%f \n  ",scm,mce);

    // rtapi_print(" --------pmMovcInit------- : uCenterStart =%f   %f  %f  \n  ",uCenterStart.x,uCenterStart.y,uCenterStart.z);
    // rtapi_print(" --------pmMovcInit------- : uCenterMid =%f   %f  %f \n ",uCenterMid.x,uCenterMid.y,uCenterMid.z);
    // rtapi_print(" --------pmMovcInit------- : uCenterEnd =%f   %f  %f  \n ",uCenterEnd.x,uCenterEnd.y,uCenterEnd.z);

    // if(fabs(theta - PM_PI)>TP_MIN_ARC_ANGLE){
    // 	pmCartCartCross(&uCenterStart, &uCenterEnd, &normal);
    // }
    /*
	if(fabs(scm - PM_PI)>TP_MIN_ARC_ANGLE){
		pmCartCartCross(&uCenterStart, &uCenterMid, &normal);
	}
	else if(fabs(mce - PM_PI)>TP_MIN_ARC_ANGLE){
		pmCartCartCross(&uCenterMid, &uCenterEnd, &normal);
		use_sm = 0;
	}
	else{
		return TP_ERR_FAIL;
	}
	*/
    if (fabs(mce - PM_PI) > TP_MIN_ARC_ANGLE)
    {
        pmCartCartCross(&uCenterMid, &uCenterEnd, &normal);
    }
    else
    {
        pmCartCartCross(&uCenterEnd, &uCenterStart, &normal);
    }
    pmCartMag(&normal, &normalMag);

    if (normalMag < TP_MIN_ARC_ANGLE)
    {
        return TP_ERR_FAIL;
    }

    normal.x /= normalMag;
    normal.y /= normalMag;
    normal.z /= normalMag;

    // 计算旋转方向
    pmCartCartCross(&uCenterStart, &uCenterMid, &normal1);
    pmCartCartCross(&uCenterStart, &uCenterEnd, &normal2);
    pmCartUnitEq(&normal1);
    pmCartUnitEq(&normal2);
    // rtapi_print("normal1 = %f %f %f \n ",normal1.x,normal1.y,normal1.z);
    // rtapi_print("normal2 = %f %f %f \n ",normal2.x,normal2.y,normal2.z);
    if (fabs(normal1.x - normal2.x) < TP_MIN_ARC_ANGLE && fabs(normal1.y - normal2.y) < TP_MIN_ARC_ANGLE && fabs(normal1.z - normal2.z) < TP_MIN_ARC_ANGLE)
    {
        //
        if (scm > sce)
        {
            if (use_sm)
            {
                pmCartCartCross(&uCenterMid, &uCenterStart, &normal);
            }
            else
            {
                pmCartCartCross(&uCenterEnd, &uCenterMid, &normal);
            }
            theta = PM_2_PI - theta;
        }
    }
    else
    {
        theta = PM_2_PI - theta;
    }
    pmCartUnitEq(&normal);
    // rtapi_print(" --------pmMovcInit------- : normal =%f   %f  %f  \n  ",normal.x,normal.y,normal.z);

    rpyStart.y = start->a * PM_PI / 180.0;
    rpyStart.p = start->b * PM_PI / 180.0;
    rpyStart.r = start->c * PM_PI / 180.0;

    rpyEnd.y = end->a * PM_PI / 180.0;
    rpyEnd.p = end->b * PM_PI / 180.0;
    rpyEnd.r = end->c * PM_PI / 180.0;

    pmRpyMatConvert(&rpyStart, &rotStart);
    pmRpyMatConvert(&rpyEnd, &rotEnd);

    pmMatInv(&rotStart, &startInv);
    // pmMatMatMult(&startInv, &rotEnd,&rot);
    pmMatMatMult(&rotEnd, &startInv, &rot);

    pmMatRotConvert(&rot, &abc);

    // rtapi_print(" --------pmMovcInit------- : abc = %f   %f   %f    %f   \n  ",abc.s,abc.x,abc.y,abc.z);

    circle->abc = abc;
    circle->normal = normal;
    circle->radius = radius;
    circle->start.tran = startPosition;
    circle->start.rot = rotStart;
    circle->end.tran = endPosition;
    circle->end.rot = rotEnd;
    circle->center = center;
    //circle->turn = 1;
    circle->angle = theta;
    return 0;
}

int get_movc_pos(MovcStruct* circle, const double portion, ZucPose* discrete_cartpos)
{
    //圆心角(弧度) * 当前弧长/总弧长
    PmRotationVector rotK;
    PmRotationMatrix rot;
    PmRotationMatrix rotEnd;
    PmRpy rpyEnd;
    rotK.s = circle->angle * portion;
    rotK.x = circle->normal.x;
    rotK.y = circle->normal.y;
    rotK.z = circle->normal.z;
    PmCartesian p1, p2, xyz;
    // get position
    pmCartCartSub(&(circle->start.tran), &(circle->center), &p1);
    pmRotMatConvert(&rotK, &rot);
    pmMatCartMult(&rot, &p1, &p2);
    pmCartCartAdd(&p2, &(circle->center), &xyz);
    discrete_cartpos->tran = xyz;
    // get gesture
    PmRotationMatrix RotStart, RotEnd;                                  //起始点、结束点
    memcpy(&RotStart, &(circle->start.rot), sizeof(PmRotationMatrix));  //起始点旋转矩阵
    memcpy(&RotEnd, &(circle->end.rot), sizeof(PmRotationMatrix));      //结束点旋转矩阵
    quaternion_interp(&RotStart, &RotEnd, portion, discrete_cartpos);
    return 0;
}

int quaternion_interp(PmRotationMatrix* const Rot_0, PmRotationMatrix* const Rot_1, const double t, ZucPose* discrete_cartpos)
{
    double cos_angle = 0.0;
    double q_0[4] = {0};
    double q_1[4] = {0};
    double q_0_s[4] = {0};  // inv of q_0
    double q_1_s[4] = {0};  // inv of q_1
    double B[4] = {0};
    double Q_t1[4] = {0};
    double Q_t2[4] = {0};
    double Q_t3[4] = {0};
    double Q_1[4] = {0};
    double Q_2[4] = {0};
    double Q_3[4] = {0};
    double temp1[4] = {0};
    double temp2[4] = {0};
    double q_t[4] = {0};
    rot2quat(Rot_0, q_0);
    rot2quat(Rot_1, q_1);
    cos_angle = q_0[0] * q_1[0] + q_0[1] * q_1[1] + q_0[2] * q_1[2] + q_0[3] * q_1[3];
    if (cos_angle < 0)
    {
        for (int i = 0; i < 4; i++) { q_1[i] = -q_1[i]; }
    }
    quartic_basic(t, B);
    get_quat_inv(q_0, q_0_s);
    get_quat_inv(q_1, q_1_s);

    quat_quat_mult(q_0_s, q_0, Q_1);
    quat_quat_mult(q_0_s, q_1, Q_2);
    quat_quat_mult(q_1_s, q_1, Q_3);

    power_quat(Q_1, B[1], Q_t1);
    power_quat(Q_2, B[2], Q_t2);
    power_quat(Q_3, B[3], Q_t3);

    quat_quat_mult(q_0, Q_t1, temp1);
    quat_quat_mult(temp1, Q_t2, temp2);
    quat_quat_mult(temp2, Q_t3, q_t);
    PmRpy Rpy_t;
    quat_rpy_convert(q_t, &Rpy_t);
    discrete_cartpos->a = Rpy_t.y * 180.0 / PM_PI;
    discrete_cartpos->b = Rpy_t.p * 180.0 / PM_PI;
    discrete_cartpos->c = Rpy_t.r * 180.0 / PM_PI;

    return 0;
}

void rot2quat(PmRotationMatrix* const m, double* q)
{
    double RotM[9] = {0};
    int flag = 0;
    double max_square = 0.0;
    double max_ref = 0.0;
    double mult = 0.0;
    double w = 0.0;
    double x = 0.0;
    double y = 0.0;
    double z = 0.0;
    double w_s = 0.0;
    double x_s = 0.0;
    double y_s = 0.0;
    double z_s = 0.0;

    RotM[0] = m->x.x;
    RotM[1] = m->y.x;
    RotM[2] = m->z.x;
    RotM[3] = m->x.y;
    RotM[4] = m->y.y;
    RotM[5] = m->z.y;
    RotM[6] = m->x.z;
    RotM[7] = m->y.z;
    RotM[8] = m->z.z;

    w_s = RotM[0] + RotM[4] + RotM[8];
    x_s = RotM[0] - RotM[4] - RotM[8];
    y_s = RotM[4] - RotM[0] - RotM[8];
    z_s = RotM[8] - RotM[0] - RotM[4];
    max_square = w_s;
    if (x_s > max_square)
    {
        max_square = x_s;
        flag = 1;
    }

    if (y_s > max_square)
    {
        max_square = y_s;
        flag = 2;
    }

    if (z_s > max_square)
    {
        max_square = z_s;
        flag = 3;
    }
    max_ref = 0.5 * sqrt(max_square + 1);
    mult = 0.25 / max_ref;
    switch (flag)
    {
    case 0:
        w = max_ref;
        x = mult * (RotM[5] - RotM[7]);
        y = mult * (RotM[6] - RotM[2]);
        z = mult * (RotM[1] - RotM[3]);
        break;

    case 1:
        x = max_ref;
        w = mult * (RotM[5] - RotM[7]);
        y = mult * (RotM[1] + RotM[3]);
        z = mult * (RotM[6] + RotM[2]);
        break;

    case 2:
        y = max_ref;
        w = mult * (RotM[6] - RotM[2]);
        x = mult * (RotM[1] + RotM[3]);
        z = mult * (RotM[5] + RotM[7]);
        break;

    case 3:
        z = max_ref;
        w = mult * (RotM[1] - RotM[3]);
        x = mult * (RotM[6] + RotM[2]);
        y = mult * (RotM[5] + RotM[7]);
        break;
    }

    if (w > 0 || w == 0)
    {
        q[0] = w;
        q[1] = x;
        q[2] = y;
        q[3] = z;
    }
    else
    {
        q[0] = -w;
        q[1] = -x;
        q[2] = -y;
        q[3] = -z;
    }
}

int get_line_position(ZucPose* start, double* dist_unit_vector, const double len, ZucPose* line_position)
{
    line_position->tran.x = start->tran.x + dist_unit_vector[0] * len;
    line_position->tran.y = start->tran.y + dist_unit_vector[1] * len;
    line_position->tran.z = start->tran.z + dist_unit_vector[2] * len;
    return 0;
}

int get_line_gesture(ZucPose* start, ZucPose* end, const double len, ZucPose* gesture)
{
    int i;
    double start_r, start_p, start_y, end_r, end_p, end_y;
    double start_quat[4] = {0};
    double end_quat[4] = {0};
    double start_quat_inv[4] = {0};
    double end_quat_inv[4] = {0};
    double delta_quat[4] = {0};
    double cos_angle = 0.0;
    double B[4] = {0};
    double Q_t1[4] = {0};
    double Q_t2[4] = {0};
    double Q_t3[4] = {0};
    double Q_1[4] = {0};
    double Q_2[4] = {0};
    double Q_3[4] = {0};
    double temp1[4] = {0};
    double temp2[4] = {0};
    double q_t[4] = {0};
    PmRotationMatrix* Rot_t;
    PmRpy Rpy_t;

    start_y = start->a * PM_PI / 180.0;  // x_axis
    start_p = start->b * PM_PI / 180.0;  // y_axis
    start_r = start->c * PM_PI / 180.0;  // z_axis

    end_y = end->a * PM_PI / 180.0;  // x_axis
    end_p = end->b * PM_PI / 180.0;
    end_r = end->c * PM_PI / 180.0;

    rpy_quat_convert(start_r, start_p, start_y, start_quat);
    rpy_quat_convert(end_r, end_p, end_y, end_quat);
    // rtapi_print("start_quat = %f %f %f %f, end_quat = %f %f %f %f\n", start_quat[0], start_quat[1], start_quat[2], start_quat[3], end_quat[0], end_quat[1], end_quat[2], end_quat[3]);

    cos_angle = start_quat[0] * end_quat[0] + start_quat[1] * end_quat[1] + start_quat[2] * end_quat[2] + start_quat[3] * end_quat[3];
    if (cos_angle < 0)
    {
        for (i = 0; i < 4; i++) { end_quat[i] = -end_quat[i]; }
    }
    /*四次多项式插补*/
    // quartic_basic(len, B);
    // get_quat_inv(start_quat, start_quat_inv);
    // get_quat_inv(end_quat, end_quat_inv);
    // quat_quat_mult(start_quat_inv, start_quat, Q_1);
    // quat_quat_mult(start_quat_inv, end_quat, Q_2);
    // quat_quat_mult(end_quat_inv, end_quat, Q_3);
    // power_quat(Q_1, B[1], Q_t1);
    // power_quat(Q_2, B[2], Q_t2);
    // power_quat(Q_3, B[3], Q_t3);
    // quat_quat_mult(start_quat, Q_t1, temp1);
    // quat_quat_mult(temp1, Q_t2, temp2);
    // quat_quat_mult(temp2, Q_t3, q_t);

    /*slerp*/
    get_quat_inv(start_quat, start_quat_inv);
    quat_quat_mult(end_quat, start_quat_inv, delta_quat);
    double delta_Q_t[4] = {0};
    power_quat(delta_quat, len, delta_Q_t);
    quat_quat_mult(delta_Q_t, start_quat, q_t);

    quat_rpy_convert(q_t, &Rpy_t);
    gesture->a = Rpy_t.y * 180.0 / PM_PI;
    gesture->b = Rpy_t.p * 180.0 / PM_PI;
    gesture->c = Rpy_t.r * 180.0 / PM_PI;
    return 0;
}

int rpy_quat_convert(double r, double p, double y, double* quat)
{
    PmRotationMatrix m;
    double sa, sb, sg;
    double ca, cb, cg;

    // sa = sin(r);
    // sb = sin(p);
    // sg = sin(y);

    // ca = cos(r);
    // cb = cos(p);
    // cg = cos(y);

    if (fabs(fabs(r) - PM_PI) <= 1e-6)
    {
        sa = 0.0;
        ca = -1.0;
    }
    else if (fabs(r) <= 1e-6)
    {
        sa = 0.0;
        ca = 1.0;
    }
    else
    {
        sa = sin(r);
        ca = cos(r);
    }

    if (fabs(fabs(p) - PM_PI) <= 1e-6)
    {
        sb = 0.0;
        cb = -1.0;
    }
    else if (fabs(p) <= 1e-6)
    {
        sb = 0.0;
        cb = 1.0;
    }
    else
    {
        sb = sin(p);
        cb = cos(p);
    }

    if (fabs(fabs(y) - PM_PI) <= 1e-6)
    {
        sg = 0.0;
        cg = -1.0;
    }
    else if (fabs(y) <= 1e-6)
    {
        sg = 0.0;
        cg = 1.0;
    }
    else
    {
        sg = sin(y);
        cg = cos(y);
    }

    m.x.x = ca * cb;
    m.y.x = ca * sb * sg - sa * cg;
    m.z.x = ca * sb * cg + sa * sg;

    m.x.y = sa * cb;
    m.y.y = sa * sb * sg + ca * cg;
    m.z.y = sa * sb * cg - ca * sg;

    m.x.z = -sb;
    m.y.z = cb * sg;
    m.z.z = cb * cg;

    double RotM[9] = {0};
    int flag = 0;
    double max_square = 0.0;
    double max_ref = 0.0;
    double mult = 0.0;
    double w = 0.0;
    double x = 0.0;
    y = 0.0;
    double z = 0.0;
    double w_s = 0.0;
    double x_s = 0.0;
    double y_s = 0.0;
    double z_s = 0.0;

    RotM[0] = m.x.x;
    RotM[1] = m.y.x;
    RotM[2] = m.z.x;
    RotM[3] = m.x.y;
    RotM[4] = m.y.y;
    RotM[5] = m.z.y;
    RotM[6] = m.x.z;
    RotM[7] = m.y.z;
    RotM[8] = m.z.z;

    w_s = RotM[0] + RotM[4] + RotM[8];
    x_s = RotM[0] - RotM[4] - RotM[8];
    y_s = RotM[4] - RotM[0] - RotM[8];
    z_s = RotM[8] - RotM[0] - RotM[4];
    max_square = w_s;

    if (x_s > max_square)
    {
        max_square = x_s;
        flag = 1;
    }

    if (y_s > max_square)
    {
        max_square = y_s;
        flag = 2;
    }

    if (z_s > max_square)
    {
        max_square = z_s;
        flag = 3;
    }

    max_ref = 0.5 * sqrt(max_square + 1);
    mult = 0.25 / max_ref;

    switch (flag)
    {
    case 0:
        w = max_ref;
        x = mult * (RotM[5] - RotM[7]);
        y = mult * (RotM[6] - RotM[2]);
        z = mult * (RotM[1] - RotM[3]);
        break;

    case 1:
        x = max_ref;
        w = mult * (RotM[5] - RotM[7]);
        y = mult * (RotM[1] + RotM[3]);
        z = mult * (RotM[6] + RotM[2]);
        break;

    case 2:
        y = max_ref;
        w = mult * (RotM[6] - RotM[2]);
        x = mult * (RotM[1] + RotM[3]);
        z = mult * (RotM[5] + RotM[7]);
        break;

    case 3:
        z = max_ref;
        w = mult * (RotM[1] - RotM[3]);
        x = mult * (RotM[6] + RotM[2]);
        y = mult * (RotM[5] + RotM[7]);
        break;
    }

    if (w > 0 || w == 0)
    {
        quat[0] = w;
        quat[1] = x;
        quat[2] = y;
        quat[3] = z;
    }
    else
    {
        quat[0] = -w;
        quat[1] = -x;
        quat[2] = -y;
        quat[3] = -z;
    }
    // quat[0] = 0.5 * pmSqrt(1.0 + m.x.x + m.y.y + m.z.z);
    // if (fabs(quat[0]) > CLOSE_TO_ZERO) {
    //     quat[1] = (m.y.z - m.z.y) / 4 / quat[0];
    //     quat[2] = (m.z.x - m.x.z) / 4 / quat[0];
    //     quat[3] = (m.x.y - m.y.x) / 4 / quat[0];
    // } else {
    //     quat[0] = 0;
    //     quat[1] = pmSqrt(1.0 + m.x.x - m.y.y - m.z.z) / 2.0;
    //     quat[2] = pmSqrt(1.0 + m.y.y - m.x.x - m.z.z) / 2.0;
    //     quat[3] = pmSqrt(1.0 + m.z.z - m.y.y - m.x.x) / 2.0;

    //     if (quat[1] > quat[2] && quat[1] > quat[3]) {
    //         if (m.x.y < 0.0) {
    //             quat[2] *= -1;
    //         }
    //         if (m.x.z < 0.0) {
    //             quat[3] *= -1;
    //         }
    //     } else if (quat[2] > quat[3]) {
    //         if (m.x.y < 0.0) {
    //             quat[1] *= -1;
    //         }
    //         if (m.y.z < 0.0) {
    //             quat[3] *= -1;
    //         }
    //     } else {
    //         if (m.x.z < 0.0) {
    //             quat[1] *= -1;
    //         }
    //         if (m.y.z < 0.0) {
    //             quat[2] *= -1;
    //         }
    //     }
    // }
    return 0;
}

void quartic_basic(const double t, double* B)
{
    double b0 = 0.0;
    double b1 = 0.0;
    double b2 = 0.0;
    double b3 = 0.0;
    double B0 = 0.0;
    double B1 = 0.0;
    double B2 = 0.0;
    double B3 = 0.0;

    b0 = -0.5 * t + 1.5 * pow(t, 2) - 1.5 * pow(t, 3) + 0.5 * pow(t, 4);
    b1 = 1 - 3 * pow(t, 2) + 2.5 * pow(t, 3) - 0.5 * pow(t, 4);
    b2 = 0.5 * t + 1.5 * pow(t, 2) - 0.5 * pow(t, 3) - 0.5 * pow(t, 4);
    b3 = -0.5 * pow(t, 3) + 0.5 * pow(t, 4);

    B0 = b0 + b1 + b2 + b3;
    B1 = b1 + b2 + b3;
    B2 = b2 + b3;
    B3 = b3;

    B[0] = B0;
    B[1] = B1;
    B[2] = B2;
    B[3] = B3;
}

int get_quat_inv(double* quat, double* quat_inv)
{
    quat_inv[0] = quat[0];
    quat_inv[1] = -quat[1];
    quat_inv[2] = -quat[2];
    quat_inv[3] = -quat[3];

    return 0;
}

int quat_quat_mult(double* quat1, double* quat2, double* quat_out)
{
    quat_out[0] = quat1[0] * quat2[0] - quat1[1] * quat2[1] - quat1[2] * quat2[2] - quat1[3] * quat2[3];
    if (quat_out[0] >= 0.0)
    {
        quat_out[1] = quat1[0] * quat2[1] + quat1[1] * quat2[0] + quat1[2] * quat2[3] - quat1[3] * quat2[2];
        quat_out[2] = quat1[0] * quat2[2] - quat1[1] * quat2[3] + quat1[2] * quat2[0] + quat1[3] * quat2[1];
        quat_out[3] = quat1[0] * quat2[3] + quat1[1] * quat2[2] - quat1[2] * quat2[1] + quat1[3] * quat2[0];
    }
    else
    {
        quat_out[0] = -quat_out[0];
        quat_out[1] = -(quat1[0] * quat2[1] + quat1[1] * quat2[0] + quat1[2] * quat2[3] - quat1[3] * quat2[2]);
        quat_out[2] = -(quat1[0] * quat2[2] - quat1[1] * quat2[3] + quat1[2] * quat2[0] + quat1[3] * quat2[1]);
        quat_out[3] = -(quat1[0] * quat2[3] + quat1[1] * quat2[2] - quat1[2] * quat2[1] + quat1[3] * quat2[0]);
    }
    return 0;
}

void power_quat(const double* q, const double t, double* Q)
{
    double Q_log[4] = {0};
    log_quat(q, Q_log);
    for (int i = 0; i < 4; i++) { Q_log[i] = Q_log[i] * t; }
    exp_quat(Q_log, Q);
}

void log_quat(const double* q, double* Q)
{
    double theta = 0.0;
    double s_theta = 0.0;
    if (q[0] > (1 - 1e-8))
    {
        Q[0] = 0.0;
        Q[1] = 0.0;
        Q[2] = 0.0;
        Q[3] = 0.0;
    }
    else
    {
        theta = acos(q[0]);
        s_theta = theta / sin(theta);
        Q[0] = 0.0;
        Q[1] = s_theta * q[1];
        Q[2] = s_theta * q[2];
        Q[3] = s_theta * q[3];
    }
}

void exp_quat(const double* q, double* Q)
{
    double q_n = 0.0;
    double s_theta = 0.0;
    double c_theta = 0.0;
    double temp = 0.0;
    norm_quat(q, &q_n);

    if (q_n < 1e-8)
    {
        Q[0] = 1.0;
        Q[1] = 0.0;
        Q[2] = 0.0;
        Q[3] = 0.0;
    }
    else
    {
        temp = 1 / q_n;
        s_theta = sin(q_n);
        c_theta = cos(q_n);
        Q[0] = c_theta;
        Q[1] = s_theta * temp * q[1];
        Q[2] = s_theta * temp * q[2];
        Q[3] = s_theta * temp * q[3];
    }
    //cout << q[0] << "\t";  cout << q[1] << "\t"; cout << q[2] << "\t"; cout << q[3] << endl;
    //cout << q_n << endl;
}

void norm_quat(const double* q, double* q_n) { *q_n = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); }

int quat_rpy_convert(double* quat, PmRpy* const rpy)
{
    PmRotationMatrix m;
    double s = 0.0;
    double x = 0.0;
    double y = 0.0;
    double z = 0.0;
    s = quat[0];
    x = quat[1];
    y = quat[2];
    z = quat[3];
    m.x.x = 1 - 2 * (y * y + z * z);
    m.y.x = 2 * (x * y + s * z);
    m.z.x = 2 * (x * z - s * y);
    m.x.y = 2 * (x * y - s * z);
    m.y.y = 1 - 2 * (x * x + z * z);
    m.z.y = 2 * (y * z + s * x);
    m.x.z = 2 * (x * z + s * y);
    m.y.z = 2 * (y * z - s * x);
    m.z.z = 1 - 2 * (x * x + y * y);

    double sy;
    sy = pmSqrt((m.x.x * m.x.x) + (m.x.y * m.x.y));
    rpy->p = atan2(-m.x.z, sy);

    if (fabs(rpy->p - (PM_PI / 2.0)) < RPY_P_FUZZ)
    {
        rpy->r = 0.0; /* force it */
        rpy->p = PM_PI / 2.0;
        rpy->y = atan2(m.y.x, m.y.y);
    }
    else if (fabs(rpy->p + (PM_PI / 2.0)) < RPY_P_FUZZ)
    {
        rpy->r = -0.0;
        rpy->p = -PM_PI / 2.0; /* force it */
        rpy->y = -atan2(m.y.x, m.y.y);
    }
    else
    {
        rpy->r = atan2(m.x.y / cos(rpy->p), m.x.x / cos(rpy->p));
        rpy->y = atan2(m.y.z / cos(rpy->p), m.z.z / cos(rpy->p));
    }
    return 0;
}

int ClearBlendQueue()
{
    if (blend_seg_jpos.jpos != NULL)
    {
        blend_seg_jpos.front = 0;
        blend_seg_jpos.rear = 0;
        rtapi_print("succeed init blend_seg_jpos space\n");
    }
    else
    {
        rtapi_print("[ClearBlendQueue]  --->  blend_seg_jpos == NULL\n");
        return -1;
    }
    return 0;
}

int BlendEnqueue(BlendJpos element_added)
{
    if ((blend_seg_jpos.rear + 1) % MAX_BLEND_LEN == blend_seg_jpos.front)
    {
        rtapi_print("队列已满！\n");
        return -1;
    }
    blend_seg_jpos.jpos[blend_seg_jpos.rear] = element_added;
    blend_seg_jpos.rear = (blend_seg_jpos.rear + 1) % MAX_BLEND_LEN;
    return 0;
}

int GetBlendFront(BlendJpos* element_front)
{
    if (blend_seg_jpos.rear == blend_seg_jpos.front)
    {
        rtapi_print("[GetBlendFront] Empty Queue!\n");
        return -1;
    }
    *element_front = blend_seg_jpos.jpos[blend_seg_jpos.front];
    blend_seg_jpos.front = (blend_seg_jpos.front + 1) % MAX_BLEND_LEN;
    return 0;
}

int get_pathvalue_coefs_pycubic(const int discrete_num, const ZucJPose* jpos_for_fit)
{
#if EN_TOPPRA
    int i, j, k;
    double delta_jpos = 0.0, pathdata = 0.0;
    // double c[4][discrete_num - 1];
    PyObject* jpos = PyList_New(discrete_num);
    PyObject* path_data = PyList_New(discrete_num);
    PyObject* list_c0 = PyList_New(discrete_num - 1);
    PyObject* list_c1 = PyList_New(discrete_num - 1);
    PyObject* list_c2 = PyList_New(discrete_num - 1);
    PyObject* list_c3 = PyList_New(discrete_num - 1);

    PyList_SetItem(path_data, 0, Py_BuildValue("d", pathdata));
    // rtapi_print("pathdata(0) = %f\n", pathdata);

    zucmotCommand.path_info[0].ss = pathdata;
    zucmotCommand.path_info[0].ss_seg_start = pathdata;
    double joint_delta = 0.0;
    for (i = 1; i < discrete_num; i++)
    {
        PyList_SetItem(list_c0, (i - 1), Py_BuildValue("d", 0.0));
        PyList_SetItem(list_c1, (i - 1), Py_BuildValue("d", 0.0));
        PyList_SetItem(list_c2, (i - 1), Py_BuildValue("d", 0.0));
        PyList_SetItem(list_c3, (i - 1), Py_BuildValue("d", 0.0));
        // if(motion_type == TC_JOINT){
        //     delta_jpos = fabs(jpos_for_fit[i].jpos[zucmotCommand.joint] - jpos_for_fit[i - 1].jpos[zucmotCommand.joint]);
        // } else {
        //     /* Cartesian space motion  */
        delta_jpos = 0.0;
        int max_num = 0;
        for (j = 0; j < zucStatus->motion.robot_joint_num; j++)
        {
            joint_delta = fabs(jpos_for_fit[i].jpos[j] - jpos_for_fit[i - 1].jpos[j]);
            if (delta_jpos < joint_delta)
            {
                delta_jpos = joint_delta;
                max_num = j;
            }
        }
        if (fabs(delta_jpos - 0.0) < 1e-6)
        {
            rtapi_print("jpos_for_fit[%d] = %f %f %f %f %f %f, jpos_for_fit[i-1] = %f %f %f %f %f %f\n",
                        i,
                        jpos_for_fit[i].jpos[0],
                        jpos_for_fit[i].jpos[1],
                        jpos_for_fit[i].jpos[2],
                        jpos_for_fit[i].jpos[3],
                        jpos_for_fit[i].jpos[4],
                        jpos_for_fit[i].jpos[5],
                        jpos_for_fit[i - 1].jpos[0],
                        jpos_for_fit[i - 1].jpos[1],
                        jpos_for_fit[i - 1].jpos[2],
                        jpos_for_fit[i - 1].jpos[3],
                        jpos_for_fit[i - 1].jpos[4],
                        jpos_for_fit[i - 1].jpos[5]);
        }
        // }
        double delta_gp = SS_INCRE * delta_jpos / zucmotCommand.disjlen;
        pathdata = pathdata + delta_gp;
        // pathdata = pathdata + SS_INCRE;
        zucmotCommand.path_info[i].ss = pathdata;
        zucmotCommand.path_info[i].ss_seg_start = pathdata;

        PyList_SetItem(path_data, i, Py_BuildValue("d", pathdata));
        if ((cur_tc_blend_r > SMALL) && (i == (zucmotCommand.cur_blend_len - REFIT_NUM)))
        {
            last_final_pathp = pathdata;
            // rtapi_print("last_final_pathp = %f\n", last_final_pathp);
        }
        // rtapi_print("path_data = %f, delta_jpos = %f, max_num = %d, zucmotCommand.disjlen = %f\n", pathdata, delta_jpos, max_num, zucmotCommand.disjlen);
    }
    zucmotCommand.path_info[discrete_num - 1].ss_seg_start = zucmotCommand.path_info[discrete_num - 2].ss;
    // rtapi_print(" *********************** cur_blend_len = %d, discrete_num = %d, cur_last_blend_len = %d, final_ss = %f *********************** \n", zucmotCommand.cur_blend_len, discrete_num, zucmotCommand.cur_last_blend_len, pathdata);

    PyObject* coefs_a;
    PyObject* coefs_b;
    PyObject* coefs_c;
    PyObject* coefs_d;
    for (int jnum = 0; jnum < zucStatus->motion.robot_joint_num; jnum++)
    {
        for (i = 0; i < discrete_num; i++) { PyList_SetItem(jpos, i, Py_BuildValue("d", jpos_for_fit[i].jpos[jnum])); }
        if (callPyCubic(path_data, jpos, list_c0, list_c1, list_c2, list_c3, discrete_num) < 0)
        {
            rtapi_print("Error in Python code, exception was printed, jnum = %d.\n", jnum);
        }
        else
        {
            for (k = 0; k < discrete_num - 1; k++)
            {
                coefs_a = PyList_GetItem(list_c0, k);
                coefs_b = PyList_GetItem(list_c1, k);
                coefs_c = PyList_GetItem(list_c2, k);
                coefs_d = PyList_GetItem(list_c3, k);
                zucmotCommand.path_info[k].pathq_coefs[jnum][0] = PyFloat_AsDouble(coefs_a);
                zucmotCommand.path_info[k].pathq_coefs[jnum][1] = PyFloat_AsDouble(coefs_b);
                zucmotCommand.path_info[k].pathq_coefs[jnum][2] = PyFloat_AsDouble(coefs_c);
                zucmotCommand.path_info[k].pathq_coefs[jnum][3] = PyFloat_AsDouble(coefs_d);
                // if(k == zucmotCommand.cur_blend_len){
                //     rtapi_print("zucmotCommand.path_info[k].ss = %f\n", zucmotCommand.path_info[k].ss);
                // }
            }
            k = discrete_num - 1;
            zucmotCommand.path_info[k].pathq_coefs[jnum][0] = zucmotCommand.path_info[k - 1].pathq_coefs[jnum][0];
            zucmotCommand.path_info[k].pathq_coefs[jnum][1] = zucmotCommand.path_info[k - 1].pathq_coefs[jnum][1];
            zucmotCommand.path_info[k].pathq_coefs[jnum][2] = zucmotCommand.path_info[k - 1].pathq_coefs[jnum][2];
            zucmotCommand.path_info[k].pathq_coefs[jnum][3] = zucmotCommand.path_info[k - 1].pathq_coefs[jnum][3];
        }
    }

    // for (i = 0; i < discrete_num; i++)
    // {
    //     rtapi_print("++++%dth, ss = %f, ss_seg_start = %f, coefs:\n", i, zucmotCommand.path_info[i].ss, zucmotCommand.path_info[i].ss_seg_start);
    //     rtapi_print("****%dth coefs:\n", i);
    //     rtapi_print("coefs[0]: %f %f %f %f %f %f\n",
    //                 zucmotCommand.path_info[i].pathq_coefs[0][0],
    //                 zucmotCommand.path_info[i].pathq_coefs[1][0],
    //                 zucmotCommand.path_info[i].pathq_coefs[2][0],
    //                 zucmotCommand.path_info[i].pathq_coefs[3][0],
    //                 zucmotCommand.path_info[i].pathq_coefs[4][0],
    //                 zucmotCommand.path_info[i].pathq_coefs[5][0]);
    //     rtapi_print("coefs[1]: %f %f %f %f %f %f\n",
    //                 zucmotCommand.path_info[i].pathq_coefs[0][1],
    //                 zucmotCommand.path_info[i].pathq_coefs[1][1],
    //                 zucmotCommand.path_info[i].pathq_coefs[2][1],
    //                 zucmotCommand.path_info[i].pathq_coefs[3][1],
    //                 zucmotCommand.path_info[i].pathq_coefs[4][1],
    //                 zucmotCommand.path_info[i].pathq_coefs[5][1]);
    //     rtapi_print("coefs[2]: %f %f %f %f %f %f\n",
    //                 zucmotCommand.path_info[i].pathq_coefs[0][2],
    //                 zucmotCommand.path_info[i].pathq_coefs[1][2],
    //                 zucmotCommand.path_info[i].pathq_coefs[2][2],
    //                 zucmotCommand.path_info[i].pathq_coefs[3][2],
    //                 zucmotCommand.path_info[i].pathq_coefs[4][2],
    //                 zucmotCommand.path_info[i].pathq_coefs[5][2]);
    //     rtapi_print("coefs[3]: %f %f %f %f %f %f\n",
    //                 zucmotCommand.path_info[i].pathq_coefs[0][3],
    //                 zucmotCommand.path_info[i].pathq_coefs[1][3],
    //                 zucmotCommand.path_info[i].pathq_coefs[2][3],
    //                 zucmotCommand.path_info[i].pathq_coefs[3][3],
    //                 zucmotCommand.path_info[i].pathq_coefs[4][3],
    //                 zucmotCommand.path_info[i].pathq_coefs[5][3]);
    // }

    if (cur_tc_blend_r <= SMALL)
    {
        last_final_pathp = 0.0;
    }

    Py_DECREF(jpos);
    Py_DECREF(path_data);
    Py_DECREF(list_c0);
    Py_DECREF(list_c1);
    Py_DECREF(list_c2);
    Py_DECREF(list_c3);
#endif
    return 0;
}

int get_blend_pathvalue_coefs_csaps(const int discrete_num, const double start_path_data, const ZucJPose* jpos_for_fit)
{
#if EN_TOPPRA
    /***************** use python csaps *****************/
    int i = 0, j = 0, k = 0;
    double smooth_para = 0.9, pathdata = start_path_data;

    double cur_tc_refit_jpos[2 * REFIT_NUM][ZUCMOT_MAX_JOINTS] = {0};
    int index = 0;
    for (int j = 1; j <= 2 * REFIT_NUM; j++)
    {
        index = zucmotCommand.cur_last_blend_len + j;
        for (int jnum = 0; jnum < zucStatus->motion.robot_joint_num; jnum++)
        {
            cur_tc_refit_jpos[j - 1][jnum] = zucmotCommand.path_info[index].pathq_coefs[jnum][3];
        }
        // rtapi_print("%dth cur_tc_refit_jpos = %f %f %f %f %f %f\n",
        //             j,
        //             cur_tc_refit_jpos[j - 1][0],
        //             cur_tc_refit_jpos[j - 1][1],
        //             cur_tc_refit_jpos[j - 1][2],
        //             cur_tc_refit_jpos[j - 1][3],
        //             cur_tc_refit_jpos[j - 1][4],
        //             cur_tc_refit_jpos[j - 1][5]);
    }

    PyObject* jpos = PyList_New(discrete_num + 3 * REFIT_NUM);
    PyObject* path_data = PyList_New(discrete_num + 3 * REFIT_NUM);
    PyObject* w = PyList_New(discrete_num + 3 * REFIT_NUM);  //weights
    PyObject* list_c0 = PyList_New(discrete_num - 1 + 3 * REFIT_NUM);
    PyObject* list_c1 = PyList_New(discrete_num - 1 + 3 * REFIT_NUM);
    PyObject* list_c2 = PyList_New(discrete_num - 1 + 3 * REFIT_NUM);
    PyObject* list_c3 = PyList_New(discrete_num - 1 + 3 * REFIT_NUM);
    PyList_SetItem(path_data, 0, Py_BuildValue("d", pathdata));
    PyList_SetItem(w, 0, Py_BuildValue("d", MAX_NUM));
    double delta_jpos = 0.0;
    double joint_delta = 0.0;
    for (i = 1; i < discrete_num + 3 * REFIT_NUM; i++)
    {
        PyList_SetItem(list_c0, (i - 1), Py_BuildValue("d", 0.0));
        PyList_SetItem(list_c1, (i - 1), Py_BuildValue("d", 0.0));
        PyList_SetItem(list_c2, (i - 1), Py_BuildValue("d", 0.0));
        PyList_SetItem(list_c3, (i - 1), Py_BuildValue("d", 0.0));
        delta_jpos = 0.0;
        if (i < REFIT_NUM)
        {
            // rtapi_print("pathdata = %f, delta_jpos = ", pathdata);
            for (int jnum = 0; jnum < zucStatus->motion.robot_joint_num; jnum++)
            {
                joint_delta = fabs(last_tc_refit_jpos[i][jnum] - last_tc_refit_jpos[i - 1][jnum]);
                // rtapi_print(" %f ", joint_delta);
                if (delta_jpos < joint_delta)
                {
                    delta_jpos = joint_delta;
                }
            }
            // rtapi_print("\n");
        }
        else if (i == REFIT_NUM)
        {
            // rtapi_print("pathdata = %f, delta_jpos = ", pathdata);
            for (int jnum = 0; jnum < zucStatus->motion.robot_joint_num; jnum++)
            {
                joint_delta = fabs(jpos_for_fit[i - REFIT_NUM].jpos[jnum] - last_tc_refit_jpos[i - 1][jnum]);
                // rtapi_print(" %f ", joint_delta);
                if (delta_jpos < joint_delta)
                {
                    delta_jpos = joint_delta;
                }
            }
            // rtapi_print("\n");
        }
        else if (i > REFIT_NUM && i < discrete_num + REFIT_NUM)
        {
            // rtapi_print("pathdata = %f, delta_jpos = ", pathdata);
            for (int jnum = 0; jnum < zucStatus->motion.robot_joint_num; jnum++)
            {
                joint_delta = fabs(jpos_for_fit[i - REFIT_NUM].jpos[jnum] - jpos_for_fit[i - REFIT_NUM - 1].jpos[jnum]);
                // rtapi_print(" %f ", joint_delta);
                if (delta_jpos < joint_delta)
                {
                    delta_jpos = joint_delta;
                }
            }
            // rtapi_print("\n");
        }
        else if (i == discrete_num + REFIT_NUM)
        {
            // rtapi_print("pathdata = %f, delta_jpos = ", pathdata);
            for (int jnum = 0; jnum < zucStatus->motion.robot_joint_num; jnum++)
            {
                joint_delta = fabs(cur_tc_refit_jpos[i - (discrete_num + REFIT_NUM)][jnum] - jpos_for_fit[i - REFIT_NUM - 1].jpos[jnum]);
                // rtapi_print(" %f ", joint_delta);
                if (delta_jpos < joint_delta)
                {
                    delta_jpos = joint_delta;
                }
            }
            // rtapi_print("\n");
        }
        else if (i > discrete_num + REFIT_NUM)
        {
            // rtapi_print("pathdata = %f, delta_jpos = ", pathdata);
            for (int jnum = 0; jnum < zucStatus->motion.robot_joint_num; jnum++)
            {
                joint_delta = fabs(cur_tc_refit_jpos[i - (discrete_num + REFIT_NUM)][jnum] - cur_tc_refit_jpos[i - (discrete_num + REFIT_NUM) - 1][jnum]);
                // rtapi_print(" %f ", joint_delta);
                if (delta_jpos < joint_delta)
                {
                    delta_jpos = joint_delta;
                }
            }
            // rtapi_print("\n");
        }

        pathdata = pathdata + SS_INCRE * delta_jpos / zucmotCommand.disjlen;
        // rtapi_print("pathdata = %f, delta_jpos = %f\n", pathdata, delta_jpos);
        // pathdata = pathdata + SS_INCRE;
        PyList_SetItem(path_data, i, Py_BuildValue("d", pathdata));
        PyList_SetItem(w, i, Py_BuildValue("d", CLOSE_TO_ZERO));
        if (i >= (REFIT_NUM - 2) && i < (discrete_num + 2 * REFIT_NUM))
        {
            // if (i == REFIT_NUM - 2)
            // {
            //     rtapi_print("start_path_data = %f, pathdata = %f\n", start_path_data, pathdata);
            // }
            zucmotCommand.blend_path_info[i - (REFIT_NUM - 2)].ss = pathdata;
            zucmotCommand.blend_path_info[i - (REFIT_NUM - 2)].ss_seg_start = pathdata;
        }
    }
    i = (discrete_num + 2 * REFIT_NUM) - 1 - (REFIT_NUM - 2);
    // rtapi_print("[before] ---> zucmotCommand.blend_path_info[last_point].ss_seg_start = %f\n", zucmotCommand.blend_path_info[i].ss_seg_start);
    zucmotCommand.blend_path_info[i].ss_seg_start = zucmotCommand.blend_path_info[i - 1].ss;
    // rtapi_print("[after] ---> zucmotCommand.blend_path_info[last_point].ss_seg_start = %f\n", zucmotCommand.blend_path_info[i].ss_seg_start);
    for (i = 0; i <= REFIT_NUM; i++) { PyList_SetItem(w, i, Py_BuildValue("d", MAX_NUM)); }
    for (i = 0; i <= 2 * REFIT_NUM; i++) { PyList_SetItem(w, (discrete_num + 3 * REFIT_NUM - 1) - i, Py_BuildValue("d", MAX_NUM)); }

    PyObject* coefs_a;
    PyObject* coefs_b;
    PyObject* coefs_c;
    PyObject* coefs_d;

    for (int jnum = 0; jnum < zucStatus->motion.robot_joint_num; jnum++)
    {
        for (i = 0; i < REFIT_NUM; i++)
        {
            PyList_SetItem(jpos, i, Py_BuildValue("d", last_tc_refit_jpos[i][jnum]));
            // rtapi_print("jpos = %f\n", last_tc_refit_jpos[i][jnum]);
        }
        // rtapi_print("*******\n");
        for (i = 0; i < discrete_num; i++)
        {
            PyList_SetItem(jpos, (i + REFIT_NUM), Py_BuildValue("d", jpos_for_fit[i].jpos[jnum]));
            // rtapi_print("jpos = %f\n", jpos_for_fit[i].jpos[jnum]);
        }
        // rtapi_print("*******\n");
        for (i = 0; i < 2 * REFIT_NUM; i++)
        {
            PyList_SetItem(jpos, (i + discrete_num + REFIT_NUM), Py_BuildValue("d", cur_tc_refit_jpos[i][jnum]));
            // rtapi_print("jpos = %f\n", cur_tc_refit_jpos[i][jnum]);
        }
        if (callPyCsapsF(path_data, jpos, w, smooth_para, list_c0, list_c1, list_c2, list_c3, discrete_num + 3 * REFIT_NUM) < 0)
        {
            rtapi_print("Error in Python code, exception was printed, blend jnum = %d.\n", jnum);
        }
        else
        {
            for (k = REFIT_NUM - 2; k < (discrete_num + 2 * REFIT_NUM) - 1; k++)
            {
                coefs_a = PyList_GetItem(list_c0, k);
                coefs_b = PyList_GetItem(list_c1, k);
                coefs_c = PyList_GetItem(list_c2, k);
                coefs_d = PyList_GetItem(list_c3, k);
                zucmotCommand.blend_path_info[k - (REFIT_NUM - 2)].pathq_coefs[jnum][0] = PyFloat_AsDouble(coefs_a);
                zucmotCommand.blend_path_info[k - (REFIT_NUM - 2)].pathq_coefs[jnum][1] = PyFloat_AsDouble(coefs_b);
                zucmotCommand.blend_path_info[k - (REFIT_NUM - 2)].pathq_coefs[jnum][2] = PyFloat_AsDouble(coefs_c);
                zucmotCommand.blend_path_info[k - (REFIT_NUM - 2)].pathq_coefs[jnum][3] = PyFloat_AsDouble(coefs_d);
            }
            k = (discrete_num + 2 * REFIT_NUM) - 1;
            j = k - 1;
            zucmotCommand.blend_path_info[k - (REFIT_NUM - 2)].pathq_coefs[jnum][0] = zucmotCommand.blend_path_info[j - (REFIT_NUM - 2)].pathq_coefs[jnum][0];
            zucmotCommand.blend_path_info[k - (REFIT_NUM - 2)].pathq_coefs[jnum][1] = zucmotCommand.blend_path_info[j - (REFIT_NUM - 2)].pathq_coefs[jnum][1];
            zucmotCommand.blend_path_info[k - (REFIT_NUM - 2)].pathq_coefs[jnum][2] = zucmotCommand.blend_path_info[j - (REFIT_NUM - 2)].pathq_coefs[jnum][2];
            zucmotCommand.blend_path_info[k - (REFIT_NUM - 2)].pathq_coefs[jnum][3] = zucmotCommand.blend_path_info[j - (REFIT_NUM - 2)].pathq_coefs[jnum][3];
        }
    }

    // for (i = 0; i < (discrete_num + REFIT_NUM) + 2; i++)
    // {
    //     rtapi_print("++++%dth, ss = %f, ss_seg_start = %f, coefs:\n", i, zucmotCommand.blend_path_info[i].ss, zucmotCommand.blend_path_info[i].ss_seg_start);
    //     rtapi_print("coefs[0]: %f %f %f %f %f %f\n",
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[0][0],
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[1][0],
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[2][0],
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[3][0],
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[4][0],
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[5][0]);
    //     rtapi_print("coefs[1]: %f %f %f %f %f %f\n",
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[0][1],
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[1][1],
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[2][1],
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[3][1],
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[4][1],
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[5][1]);
    //     rtapi_print("coefs[2]: %f %f %f %f %f %f\n",
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[0][2],
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[1][2],
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[2][2],
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[3][2],
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[4][2],
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[5][2]);
    //     rtapi_print("coefs[3]: %f %f %f %f %f %f\n",
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[0][3],
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[1][3],
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[2][3],
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[3][3],
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[4][3],
    //                 zucmotCommand.blend_path_info[i].pathq_coefs[5][3]);
    // }
    Py_DECREF(jpos);
    Py_DECREF(path_data);
    Py_DECREF(w);
    Py_DECREF(list_c0);
    Py_DECREF(list_c1);
    Py_DECREF(list_c2);
    Py_DECREF(list_c3);
#endif
    return 0;
}

int set_constraints_toppra(ConstraintsToppra& constraints)
{
    int i;
    if (zucmotCommand.motion_type == TC_JOINT)
    {
        // constraints.req_vel[1] = trajjointmov.vel;
        double jvel = zucmotCommand.trajjointmov.vel;
        double jacc = zucmotCommand.trajjointmov.acc;
        for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
        {
            constraints.vel_lim[i][0] = -jvel;
            constraints.vel_lim[i][1] = jvel;
            constraints.acc_lim[i][0] = -jacc;
            constraints.acc_lim[i][1] = jacc;
        }
    }
    else
    {
        // constraints.req_vel[1] = zucmotCommand.trajlinearmov.vel;
        for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
        {
            constraints.vel_lim[i][0] = -JointConfig[i].MaxVel;
            constraints.vel_lim[i][1] = JointConfig[i].MaxVel;
            constraints.acc_lim[i][0] = -JointConfig[i].MaxAccel;
            constraints.acc_lim[i][1] = JointConfig[i].MaxAccel;  //traj_planning中的宏“ACC_BOUND”的值已更改为joint[0].acc_limit
        }
    }
    double fv_value = 0.0;
    for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
    {
        // rtapi_print("tor_limit = %f, vf_params = %f, MaxVel = %f ", tor_limit[i], vf_params[i], JointConfig[i].MaxVel);
        fv_value = vf_params[i] * JointConfig[i].MaxVel * PM_PI / 180.0;
        constraints.tor_lim[i][1] = tor_limit[i] * usertor_portion;  // - fv_value;
        if (constraints.tor_lim[i][1] < 2 * fv_value)
        {
            constraints.tor_lim[i][1] = 2 * fv_value;
        }
        constraints.tor_lim[i][0] = -constraints.tor_lim[i][1];
        // rtapi_print("final tor_lim = %f %f\n", constraints.tor_lim[i][1], constraints.tor_lim[i][0]);
    }

    constraints.lim_add[0] = 1;  //joint vel constraint
    constraints.lim_add[2] = 1;  //不能只添加关节速度约束
    constraints.lim_add[1] = userset_filter_size;
    return 0;
}

int get_blend_path_information(const int blend_jpos_len, ConstraintsToppra& constraints)  //计算最优化参数: vel, acc, tor
{
    int i, j, k;
    double delta_s_value;  // 用于代入到表达式中计算q(s)一阶导
    PathCoefs pathcoefs;
    for (i = 0; i < blend_jpos_len; i++)
    {
        delta_s_value = zucmotCommand.blend_path_info[i].ss - zucmotCommand.blend_path_info[i].ss_seg_start;
        for (j = 0; j < zucStatus->motion.robot_joint_num; j++)
        {
            for (k = 0; k < 4; k++) { pathcoefs.pathq_coefs[j][k] = zucmotCommand.blend_path_info[i].pathq_coefs[j][k]; }
        }
        blend_joint_vel_constraints_params(i, delta_s_value, pathcoefs, constraints);
        // if (constraints.lim_add[1] == 1)
        // {
        //     blend_joint_acc_constraints_params(i, delta_s_value, pathcoefs, constraints);
        // }
        if (constraints.lim_add[2] == 1)
        {
            blend_joint_tor_constraints_params(i, delta_s_value, pathcoefs, constraints);
        }
    }
    blend_interpolate_discretization(blend_jpos_len);

    return 0;
}

int blend_joint_vel_constraints_params(const int i_th, const double delta_s_value, PathCoefs& pathcoefs, ConstraintsToppra& constraints)  // get xbound
{
    // 根据关节速度约束计算路径速度的边界
    int i, j;
    double sd_min, sd_max;
    double q_d_s[ZUCMOT_MAX_JOINTS];  //q对s的一阶导
    sd_min = 0;
    sd_max = MAX_NUM;
    for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
    {
        q_d_s[i] = 3 * pathcoefs.pathq_coefs[i][0] * pmSq(delta_s_value) + 2 * pathcoefs.pathq_coefs[i][1] * delta_s_value + pathcoefs.pathq_coefs[i][2];
        if (q_d_s[i] > 0)
        {
            sd_max = get_min((constraints.vel_lim[i][1]) / (q_d_s[i]), sd_max);
            sd_min = get_max((constraints.vel_lim[i][0]) / (q_d_s[i]), sd_min);
        }
        if (q_d_s[i] < 0)
        {
            sd_max = get_min((constraints.vel_lim[i][0]) / (q_d_s[i]), sd_max);
            sd_min = get_max((constraints.vel_lim[i][1]) / (q_d_s[i]), sd_min);
        }
    }
    zucmotCommand.blend_path_info[i_th].xbound[0] = sd_min * sd_min;
    zucmotCommand.blend_path_info[i_th].xbound[1] = sd_max * sd_max;
    return 0;
}

int blend_joint_acc_constraints_params(const int i_th,
                                       const double delta_s_value,
                                       PathCoefs& pathcoefs,
                                       ConstraintsToppra& constraints)  // get jacc_params: jacc_a, jacc_b
{
    // int i;
    // // double q_d_s[ZUCMOT_MAX_JOINTS], q_dd_s[ZUCMOT_MAX_JOINTS];//q对s的一阶导和二阶导
    // for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
    // {
    //     zucmotCommand.blend_path_info[i_th].jacc_a[i] =
    //         3 * pathcoefs.pathq_coefs[i][0] * pmSq(delta_s_value) + 2 * pathcoefs.pathq_coefs[i][1] * delta_s_value + pathcoefs.pathq_coefs[i][2];
    //     zucmotCommand.blend_path_info[i_th].jacc_b[i] = 6 * pathcoefs.pathq_coefs[i][0] * delta_s_value + 2 * pathcoefs.pathq_coefs[i][1];
    // }
    return 0;
}

int blend_joint_tor_constraints_params(const int i_th,
                                       const double delta_s_value,
                                       PathCoefs& pathcoefs,
                                       ConstraintsToppra& constraints)  // jtor_params: jtor_a, jtor_b, jtor_c, jtor_d
{
    int i;
    double q_s[ZUCMOT_MAX_JOINTS], q_d_s[ZUCMOT_MAX_JOINTS], q_dd_s[ZUCMOT_MAX_JOINTS];
    double q_equal[ZUCMOT_MAX_JOINTS], qd_equal[ZUCMOT_MAX_JOINTS], qdd_equal[ZUCMOT_MAX_JOINTS], tor_equal[ZUCMOT_MAX_JOINTS], tg[ZUCMOT_MAX_JOINTS],
        tf[ZUCMOT_MAX_JOINTS], tfv[ZUCMOT_MAX_JOINTS], tfc[ZUCMOT_MAX_JOINTS], f_offset[ZUCMOT_MAX_JOINTS], zero_array[ZUCMOT_MAX_JOINTS];
    for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
    {
        q_s[i] = pathcoefs.pathq_coefs[i][0] * pmSq(delta_s_value) * delta_s_value + pathcoefs.pathq_coefs[i][1] * pmSq(delta_s_value) +
                 pathcoefs.pathq_coefs[i][2] * delta_s_value + pathcoefs.pathq_coefs[i][3];
        q_d_s[i] = 3 * pathcoefs.pathq_coefs[i][0] * pmSq(delta_s_value) + 2 * pathcoefs.pathq_coefs[i][1] * delta_s_value + pathcoefs.pathq_coefs[i][2];
        q_dd_s[i] = 6 * pathcoefs.pathq_coefs[i][0] * delta_s_value + 2 * pathcoefs.pathq_coefs[i][1];
    }

    for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
    {
        q_equal[i] = q_s[i] * M_PI / 180;
        qd_equal[i] = 0;
        qdd_equal[i] = 0;
        zero_array[i] = 0;
    }
    getTa_toppra(q_equal, qd_equal, qdd_equal, tor_equal, tfv, tfc, f_offset);

    for (i = 0; i < zucStatus->motion.robot_joint_num; i++) { zucmotCommand.blend_path_info[i_th].jtor_c[i] = tor_equal[i]; }

    for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
    {
        q_equal[i] = q_s[i] * M_PI / 180;
        qd_equal[i] = 0;
        qdd_equal[i] = q_d_s[i] * M_PI / 180;
    }
    getTa_toppra(q_equal, qd_equal, qdd_equal, tor_equal, tfv, tfc, f_offset);
    for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
    {
        zucmotCommand.blend_path_info[i_th].jtor_a[i] = tor_equal[i] - zucmotCommand.blend_path_info[i_th].jtor_c[i];
    }

    for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
    {
        q_equal[i] = q_s[i] * M_PI / 180;
        qd_equal[i] = q_d_s[i] * M_PI / 180;
        qdd_equal[i] = q_dd_s[i] * M_PI / 180;
    }
    getTa_toppra(q_equal, qd_equal, qdd_equal, tor_equal, tfv, tfc, f_offset);
    for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
    {
        zucmotCommand.blend_path_info[i_th].jtor_b[i] = tor_equal[i] - zucmotCommand.blend_path_info[i_th].jtor_c[i] - tfv[i] - tfc[i];
    }

    for (i = 0; i < zucStatus->motion.robot_joint_num; i++) { zucmotCommand.blend_path_info[i_th].jtor_c[i] += tfc[i]; }
    for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
    {
        zucmotCommand.blend_path_info[i_th].jtor_d[i] = vf_params[i] * q_d_s[i] * M_PI / 180;  //在计算可达集的时候使用，不过忽略粘滞摩擦力的话影响应该不算很大
    }
    return 0;
}

int blend_interpolate_discretization(const int blend_jpos_len)
{
    int i = 0, j = 0, k = 0;
    double delta = 0.0, ss_next = 0.0, delta_tar = 0.0;
    for (i = 0; i < 1; i++)
    {
        zucmotCommand.blend_path_info[i].min_time = 0.0;
        for (j = 6; j < 12; j++)
        {
            k = j - 6;
            // zucmotCommand.blend_path_info[i].jacc_a[j] = zucmotCommand.blend_path_info[i].jacc_a[k];
            // zucmotCommand.blend_path_info[i].jacc_b[j] = zucmotCommand.blend_path_info[i].jacc_b[k];

            zucmotCommand.blend_path_info[i].jtor_a[j] = zucmotCommand.blend_path_info[i].jtor_a[k];
            zucmotCommand.blend_path_info[i].jtor_b[j] = zucmotCommand.blend_path_info[i].jtor_b[k];
            zucmotCommand.blend_path_info[i].jtor_c[j] = zucmotCommand.blend_path_info[i].jtor_c[k];
            zucmotCommand.blend_path_info[i].jtor_d[j] = zucmotCommand.blend_path_info[i].jtor_d[k];
        }
    }
    //这里计算min_time的方式需要修正：因为在规划中是把转接段归于前一运动段的，这样的话，在计算min_time的时候就应该使用前一运动段的运动类型and最大速度
    double jpos1[ZUCMOT_MAX_JOINTS] = {0}, jpos2[ZUCMOT_MAX_JOINTS] = {0};
    ZucPose cartpos1, cartpos2;
    KINEMATICS_FORWARD_FLAGS fflags = 0;
    KINEMATICS_INVERSE_FLAGS iflags = 0;
    double delta_jpos = 0.0;
    double delta_tar_cart = 0.0;
    double min_time_cart = 0.0;

    for (i = 1; i < blend_jpos_len; i++)
    {
        ss_next = zucmotCommand.blend_path_info[i - 1].ss;
        delta = zucmotCommand.blend_path_info[i].ss - ss_next;

        for (j = 0; j < zucStatus->motion.robot_joint_num; j++)
        {
            jpos1[j] = zucmotCommand.blend_path_info[i - 1].pathq_coefs[j][3] + zucmotCommand.blend_path_info[i - 1].pathq_coefs[j][2] * delta +
                       zucmotCommand.blend_path_info[i - 1].pathq_coefs[j][1] * delta * delta +
                       zucmotCommand.blend_path_info[i - 1].pathq_coefs[j][0] * delta * delta * delta;
            jpos2[j] = zucmotCommand.blend_path_info[i].pathq_coefs[j][3] + zucmotCommand.blend_path_info[i].pathq_coefs[j][2] * delta +
                       zucmotCommand.blend_path_info[i].pathq_coefs[j][1] * delta * delta +
                       zucmotCommand.blend_path_info[i].pathq_coefs[j][0] * delta * delta * delta;
        }
        kinematicsForward(&(zucmotConfig.tool_offset), jpos1, &cartpos1, &fflags, &iflags, &(zucmotConfig.base_offset), &(zucmotConfig.user_offset));
        kinematicsForward(&(zucmotConfig.tool_offset), jpos2, &cartpos2, &fflags, &iflags, &(zucmotConfig.base_offset), &(zucmotConfig.user_offset));
        delta_tar_cart = pmSqrt(pmSq(cartpos1.tran.x - cartpos2.tran.x) + pmSq(cartpos1.tran.y - cartpos2.tran.y) + pmSq(cartpos1.tran.z - cartpos2.tran.z));
        if (zucmotCommand.motion_type == TC_JOINT)
        {
            delta_tar = 0.0;
            for (int jnum = 0; jnum < zucStatus->motion.robot_joint_num; jnum++)
            {
                delta_jpos = fabs(jpos1[jnum] - jpos2[jnum]);
                if (delta_tar < delta_jpos)
                {
                    delta_tar = delta_jpos;
                }
            }

            // double j1 = 0.0, j2 = 0.0, j3 = 0.0, j4 = 0.0;
            // j1 = zucmotCommand.blend_path_info[i - 1].pathq_coefs[zucmotCommand.joint][3] + zucmotCommand.blend_path_info[i - 1].pathq_coefs[zucmotCommand.joint][2] * delta +
            //         zucmotCommand.blend_path_info[i - 1].pathq_coefs[zucmotCommand.joint][1] * delta * delta + zucmotCommand.blend_path_info[i - 1].pathq_coefs[zucmotCommand.joint][0] * delta * delta * delta;
            // j2 = zucmotCommand.blend_path_info[i].pathq_coefs[zucmotCommand.joint][3] + zucmotCommand.blend_path_info[i].pathq_coefs[zucmotCommand.joint][2] * delta +
            //         zucmotCommand.blend_path_info[i].pathq_coefs[zucmotCommand.joint][1] * delta * delta + zucmotCommand.blend_path_info[i].pathq_coefs[zucmotCommand.joint][0] * delta * delta * delta;

            // j3 = zucmotCommand.blend_path_info[i - 1].pathq_coefs[lasttc_jointNum][3] + zucmotCommand.blend_path_info[i - 1].pathq_coefs[lasttc_jointNum][2] * delta +
            //         zucmotCommand.blend_path_info[i - 1].pathq_coefs[lasttc_jointNum][1] * delta * delta + zucmotCommand.blend_path_info[i - 1].pathq_coefs[lasttc_jointNum][0] * delta * delta * delta;
            // j4 = zucmotCommand.blend_path_info[i].pathq_coefs[lasttc_jointNum][3] + zucmotCommand.blend_path_info[i].pathq_coefs[lasttc_jointNum][2] * delta +
            //         zucmotCommand.blend_path_info[i].pathq_coefs[lasttc_jointNum][1] * delta * delta + zucmotCommand.blend_path_info[i].pathq_coefs[lasttc_jointNum][0] * delta * delta * delta;

            // if(fabs(j3 - j4) > fabs(j1 - j2)){
            //     delta_tar = fabs(j3 - j4);
            // } else {
            //     delta_tar = fabs(j1 - j2);
            // }

            // if (trajjointmov.ini_maxvel > V_FUZZ)
            // {  //min_time的计算需改进，主要是转接和调倍率的影响
            zucmotCommand.blend_path_info[i].min_time = delta_tar / zucmotCommand.trajjointmov.ini_maxvel;
            // }
            // else
            // {
            //     zucmotCommand.blend_path_info[i].min_time = 1e8;
            // }
            // if (tcp_vel_limit_toppra < V_FUZZ)
            // {
            //     zucmotCommand.blend_path_info[i].min_time = 1e8;
            // }
            // else
            // {
            min_time_cart = delta_tar_cart / tcp_vel_limit_toppra;
            if (min_time_cart > zucmotCommand.blend_path_info[i].min_time)
            {
                zucmotCommand.blend_path_info[i].min_time = min_time_cart;
            }
            // }
            // rtapi_print("tcp_vel_limit_toppra = %f, min_time = %f, min_time_cart = %f, delta_tar_cart = %f\n", tcp_vel_limit_toppra, zucmotCommand.path_info[i].min_time, min_time_cart, delta_tar_cart);
        }
        else
        {
            delta_tar = delta_tar_cart;
            // double cart_vel = 0.0;
            if (zucmotCommand.motion_type == TC_MOVC)
            {
                // cart_vel = trajmovc.ini_maxvel;
                // if (cart_vel > tcp_vel_limit_toppra)
                // {
                //     cart_vel = tcp_vel_limit_toppra;
                // }
                zucmotCommand.blend_path_info[i].min_time = delta_tar / zucmotCommand.trajmovc.ini_maxvel;
                // if (cart_vel > V_FUZZ)
                // {
                //     zucmotCommand.blend_path_info[i].min_time = delta_tar / cart_vel;
                // }
                // else
                // {
                //     zucmotCommand.blend_path_info[i].min_time = 1e8;
                // }
            }
            else if (zucmotCommand.motion_type == TC_LINEAR)
            {
                if (zucmotCommand.trajlinearmov.using_abc_vel)
                {
                    min_time_cart =
                        delta_tar / zucmotCommand.trajlinearmov.ini_maxvel;  //以姿态做规划主轴时，这里暂时使用轴的最大速度，后续需要改善的话再进行修改
                    if (min_time_cart > zucmotCommand.blend_path_info[i].min_time)
                    {
                        zucmotCommand.blend_path_info[i].min_time = min_time_cart;
                    }
                }
                else
                {
                    // cart_vel = zucmotCommand.trajlinearmov.ini_maxvel;
                    // if (cart_vel > tcp_vel_limit_toppra)
                    // {
                    //     cart_vel = tcp_vel_limit_toppra;
                    // }
                    // if (cart_vel > V_FUZZ)
                    // {
                    zucmotCommand.blend_path_info[i].min_time = delta_tar / zucmotCommand.trajlinearmov.ini_maxvel;
                    // }
                    // else
                    // {
                    //     zucmotCommand.blend_path_info[i].min_time = 1e8;
                    // }
                }
            }
            // rtapi_print("[blend interpolate discretization] : cartpos2 = %f %f %f %f %f %f\n", cartpos2.tran.x, cartpos2.tran.y, cartpos2.tran.z, cartpos2.a, cartpos2.b, cartpos2.c);
        }

        // rtapi_print("[blend_interpolate]: min_time = %f\n", zucmotCommand.blend_path_info[i].min_time);
        for (j = 6; j < 12; j++)
        {
            k = j - 6;
            // zucmotCommand.blend_path_info[i].jacc_a[j] =
            //     zucmotCommand.blend_path_info[i - 1].jacc_a[k] - 2 * delta * zucmotCommand.blend_path_info[i - 1].jacc_b[k];
            // zucmotCommand.blend_path_info[i].jacc_b[j] = zucmotCommand.blend_path_info[i - 1].jacc_b[k];

            zucmotCommand.blend_path_info[i].jtor_a[j] =
                zucmotCommand.blend_path_info[i - 1].jtor_a[k] - 2 * delta * zucmotCommand.blend_path_info[i - 1].jtor_b[k];
            zucmotCommand.blend_path_info[i].jtor_b[j] = zucmotCommand.blend_path_info[i - 1].jtor_b[k];
            zucmotCommand.blend_path_info[i].jtor_c[j] = zucmotCommand.blend_path_info[i - 1].jtor_c[k];
            zucmotCommand.blend_path_info[i].jtor_d[j] = zucmotCommand.blend_path_info[i - 1].jtor_d[k];
        }
    }
    return 0;
}

int get_path_information(const int jpos_len, ConstraintsToppra& constraints)  //计算最优化参数: vel, acc, tor
{
    int i, j, k;
    double delta_s_value;  // 用于代入到表达式中计算q(s)一阶导
    PathCoefs pathcoefs;
    for (i = 0; i < jpos_len; i++)
    {
        delta_s_value = zucmotCommand.path_info[i].ss - zucmotCommand.path_info[i].ss_seg_start;
        for (j = 0; j < zucStatus->motion.robot_joint_num; j++)
        {
            for (k = 0; k < 4; k++) { pathcoefs.pathq_coefs[j][k] = zucmotCommand.path_info[i].pathq_coefs[j][k]; }
        }
        joint_vel_constraints_params(i, delta_s_value, pathcoefs, constraints);
        // if (constraints.lim_add[1] == 1)
        // {
        //     joint_acc_constraints_params(i, delta_s_value, pathcoefs, constraints);
        // }
        if (constraints.lim_add[2] == 1)
        {
            joint_tor_constraints_params(i, delta_s_value, pathcoefs, constraints);
        }
    }
    interpolate_discretization(jpos_len);

    return 0;
}

int joint_vel_constraints_params(const int i_th, const double delta_s_value, PathCoefs& pathcoefs, ConstraintsToppra& constraints)  // get xbound
{
    // 根据关节速度约束计算路径速度的边界
    int i, j;
    double sd_min, sd_max;
    double q_d_s[ZUCMOT_MAX_JOINTS];  //q对s的一阶导
    sd_min = 0;
    sd_max = MAX_NUM;
    for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
    {
        q_d_s[i] = 3 * pathcoefs.pathq_coefs[i][0] * pmSq(delta_s_value) + 2 * pathcoefs.pathq_coefs[i][1] * delta_s_value + pathcoefs.pathq_coefs[i][2];
        if (q_d_s[i] > 0)
        {
            sd_max = get_min((constraints.vel_lim[i][1]) / (q_d_s[i]), sd_max);
            sd_min = get_max((constraints.vel_lim[i][0]) / (q_d_s[i]), sd_min);
        }
        if (q_d_s[i] < 0)
        {
            sd_max = get_min((constraints.vel_lim[i][0]) / (q_d_s[i]), sd_max);
            sd_min = get_max((constraints.vel_lim[i][1]) / (q_d_s[i]), sd_min);
        }
    }
    zucmotCommand.path_info[i_th].xbound[0] = sd_min * sd_min;
    zucmotCommand.path_info[i_th].xbound[1] = sd_max * sd_max;
    return 0;
}

int joint_acc_constraints_params(const int i_th,
                                 const double delta_s_value,
                                 PathCoefs& pathcoefs,
                                 ConstraintsToppra& constraints)  // get jacc_params: jacc_a, jacc_b
{
    // int i;
    // // double q_d_s[ZUCMOT_MAX_JOINTS], q_dd_s[ZUCMOT_MAX_JOINTS];//q对s的一阶导和二阶导
    // for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
    // {
    //     zucmotCommand.path_info[i_th].jacc_a[i] =
    //         3 * pathcoefs.pathq_coefs[i][0] * pmSq(delta_s_value) + 2 * pathcoefs.pathq_coefs[i][1] * delta_s_value + pathcoefs.pathq_coefs[i][2];
    //     zucmotCommand.path_info[i_th].jacc_b[i] = 6 * pathcoefs.pathq_coefs[i][0] * delta_s_value + 2 * pathcoefs.pathq_coefs[i][1];
    // }
    return 0;
}

int joint_tor_constraints_params(const int i_th,
                                 const double delta_s_value,
                                 PathCoefs& pathcoefs,
                                 ConstraintsToppra& constraints)  // jtor_params: jtor_a, jtor_b, jtor_c, jtor_d
{
    int i;
    double q_s[ZUCMOT_MAX_JOINTS], q_d_s[ZUCMOT_MAX_JOINTS], q_dd_s[ZUCMOT_MAX_JOINTS];
    double q_equal[ZUCMOT_MAX_JOINTS], qd_equal[ZUCMOT_MAX_JOINTS], qdd_equal[ZUCMOT_MAX_JOINTS], tor_equal[ZUCMOT_MAX_JOINTS], tg[ZUCMOT_MAX_JOINTS],
        tf[ZUCMOT_MAX_JOINTS], tfv[ZUCMOT_MAX_JOINTS], tfc[ZUCMOT_MAX_JOINTS], f_offset[ZUCMOT_MAX_JOINTS], zero_array[ZUCMOT_MAX_JOINTS];
    for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
    {
        q_s[i] = pathcoefs.pathq_coefs[i][0] * pmSq(delta_s_value) * delta_s_value + pathcoefs.pathq_coefs[i][1] * pmSq(delta_s_value) +
                 pathcoefs.pathq_coefs[i][2] * delta_s_value + pathcoefs.pathq_coefs[i][3];
        q_d_s[i] = 3 * pathcoefs.pathq_coefs[i][0] * pmSq(delta_s_value) + 2 * pathcoefs.pathq_coefs[i][1] * delta_s_value + pathcoefs.pathq_coefs[i][2];
        q_dd_s[i] = 6 * pathcoefs.pathq_coefs[i][0] * delta_s_value + 2 * pathcoefs.pathq_coefs[i][1];
    }

    for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
    {
        q_equal[i] = q_s[i] * M_PI / 180;
        qd_equal[i] = 0;
        qdd_equal[i] = 0;
        zero_array[i] = 0;
    }
    getTa_toppra(q_equal, qd_equal, qdd_equal, tor_equal, tfv, tfc, f_offset);
    // rtapi_print("q_equal = %f %f %f %f %f %f, tor_equal = %f %f %f %f %f %f\n", q_equal[0], q_equal[1], q_equal[2], q_equal[3], q_equal[4], q_equal[5], tor_equal[0], tor_equal[1], tor_equal[2],
    //         tor_equal[3], tor_equal[4], tor_equal[5]);

    for (i = 0; i < zucStatus->motion.robot_joint_num; i++) { zucmotCommand.path_info[i_th].jtor_c[i] = tor_equal[i]; }

    for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
    {
        q_equal[i] = q_s[i] * M_PI / 180;
        qd_equal[i] = 0;
        qdd_equal[i] = q_d_s[i] * M_PI / 180;
    }
    getTa_toppra(q_equal, qd_equal, qdd_equal, tor_equal, tfv, tfc, f_offset);
    for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
    {
        zucmotCommand.path_info[i_th].jtor_a[i] = tor_equal[i] - zucmotCommand.path_info[i_th].jtor_c[i];
    }
    // rtapi_print("jtor_a = %f %f %f %f %f %f\n", zucmotCommand.path_info[i_th].jtor_a[0], zucmotCommand.path_info[i_th].jtor_a[1], zucmotCommand.path_info[i_th].jtor_a[2],
    //         zucmotCommand.path_info[i_th].jtor_a[3], zucmotCommand.path_info[i_th].jtor_a[4], zucmotCommand.path_info[i_th].jtor_a[5]);

    for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
    {
        q_equal[i] = q_s[i] * M_PI / 180;
        qd_equal[i] = q_d_s[i] * M_PI / 180;
        qdd_equal[i] = q_dd_s[i] * M_PI / 180;
    }
    getTa_toppra(q_equal, qd_equal, qdd_equal, tor_equal, tfv, tfc, f_offset);
    for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
    {
        zucmotCommand.path_info[i_th].jtor_b[i] = tor_equal[i] - zucmotCommand.path_info[i_th].jtor_c[i] - tfv[i] - tfc[i];
    }
    // rtapi_print("jtor_b = %f %f %f %f %f %f\n", zucmotCommand.path_info[i_th].jtor_b[0], zucmotCommand.path_info[i_th].jtor_b[1], zucmotCommand.path_info[i_th].jtor_b[2],
    //         zucmotCommand.path_info[i_th].jtor_b[3], zucmotCommand.path_info[i_th].jtor_b[4], zucmotCommand.path_info[i_th].jtor_b[5]);
    for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
    {
        zucmotCommand.path_info[i_th].jtor_c[i] += tfc[i];
        zucmotCommand.path_info[i_th].jtor_c[i] *= usertor_portion;
    }
    // rtapi_print("jtor_c = %f %f %f %f %f %f\n", zucmotCommand.path_info[i_th].jtor_c[0], zucmotCommand.path_info[i_th].jtor_c[1], zucmotCommand.path_info[i_th].jtor_c[2],
    //         zucmotCommand.path_info[i_th].jtor_c[3], zucmotCommand.path_info[i_th].jtor_c[4], zucmotCommand.path_info[i_th].jtor_c[5]);
    for (i = 0; i < zucStatus->motion.robot_joint_num; i++)
    {
        zucmotCommand.path_info[i_th].jtor_d[i] = vf_params[i] * q_d_s[i] * M_PI / 180;  //在计算可达集的时候使用，不过忽略粘滞摩擦力的话影响应该不算很大
    }

    // rtapi_print("jtor_d = %f %f %f %f %f %f\n", zucmotCommand.path_info[i_th].jtor_d[0], zucmotCommand.path_info[i_th].jtor_d[1], zucmotCommand.path_info[i_th].jtor_d[2],
    //         zucmotCommand.path_info[i_th].jtor_d[3], zucmotCommand.path_info[i_th].jtor_d[4], zucmotCommand.path_info[i_th].jtor_d[5]);
    // rtapi_print("\n");
    return 0;
}

int interpolate_discretization(const int jpos_len)
{
    int i = 0, j = 0, k = 0;
    double delta = 0.0, ss_next = 0.0, delta_tar = 0.0;
    for (i = 0; i < 1; i++)
    {
        zucmotCommand.path_info[i].min_time = 0.0;
        for (j = 6; j < 12; j++)
        {
            k = j - 6;
            // zucmotCommand.path_info[i].jacc_a[j] = zucmotCommand.path_info[i].jacc_a[k];
            // zucmotCommand.path_info[i].jacc_b[j] = zucmotCommand.path_info[i].jacc_b[k];

            zucmotCommand.path_info[i].jtor_a[j] = zucmotCommand.path_info[i].jtor_a[k];
            zucmotCommand.path_info[i].jtor_b[j] = zucmotCommand.path_info[i].jtor_b[k];
            zucmotCommand.path_info[i].jtor_c[j] = zucmotCommand.path_info[i].jtor_c[k];
            zucmotCommand.path_info[i].jtor_d[j] = zucmotCommand.path_info[i].jtor_d[k];
            // rtapi_print("[task]: %dth jtor_param = %f %f %f %f\n", j, zucmotCommand.path_info[i].jtor_a[j], zucmotCommand.path_info[i].jtor_b[j], zucmotCommand.path_info[i].jtor_c[j],
            //         zucmotCommand.path_info[i].jtor_d[j]);
        }
    }

    double jpos1[ZUCMOT_MAX_JOINTS] = {0}, jpos2[ZUCMOT_MAX_JOINTS] = {0};
    ZucPose cartpos1, cartpos2;
    KINEMATICS_FORWARD_FLAGS fflags = 0;
    KINEMATICS_INVERSE_FLAGS iflags = 0;
    double delta_jpos = 0.0;
    double min_time_cart = 0.0;
    for (i = 1; i < jpos_len; i++)
    {
        ss_next = zucmotCommand.path_info[i - 1].ss;
        delta = zucmotCommand.path_info[i].ss - ss_next;
        for (j = 0; j < zucStatus->motion.robot_joint_num; j++)
        {
            jpos1[j] = zucmotCommand.path_info[i - 1].pathq_coefs[j][3] + zucmotCommand.path_info[i - 1].pathq_coefs[j][2] * delta +
                       zucmotCommand.path_info[i - 1].pathq_coefs[j][1] * delta * delta +
                       zucmotCommand.path_info[i - 1].pathq_coefs[j][0] * delta * delta * delta;
            jpos2[j] = zucmotCommand.path_info[i].pathq_coefs[j][3] + zucmotCommand.path_info[i].pathq_coefs[j][2] * delta +
                       zucmotCommand.path_info[i].pathq_coefs[j][1] * delta * delta + zucmotCommand.path_info[i].pathq_coefs[j][0] * delta * delta * delta;
        }
        kinematicsForward(&(zucmotConfig.tool_offset), jpos1, &cartpos1, &fflags, &iflags, &(zucmotConfig.base_offset), &(zucmotConfig.user_offset));
        kinematicsForward(&(zucmotConfig.tool_offset), jpos2, &cartpos2, &fflags, &iflags, &(zucmotConfig.base_offset), &(zucmotConfig.user_offset));
        // rtapi_print("jpos1 = %f, jpos2 = %f, cart1 = %f %f %f, cart2 = %f %f %f\n", jpos1[0], jpos2[0], cartpos1.tran.x, cartpos1.tran.y, cartpos1.tran.z, cartpos2.tran.x, cartpos2.tran.y, cartpos2.tran.z);
        double delta_tar_cart =
            pmSqrt(pmSq(cartpos1.tran.x - cartpos2.tran.x) + pmSq(cartpos1.tran.y - cartpos2.tran.y) + pmSq(cartpos1.tran.z - cartpos2.tran.z));

        if (zucmotCommand.motion_type == TC_JOINT)
        {
            // double j1 = 0.0, j2 = 0.0, j3 = 0.0, j4 = 0.0;
            // j1 = zucmotCommand.path_info[i - 1].pathq_coefs[zucmotCommand.joint][3] + zucmotCommand.path_info[i - 1].pathq_coefs[zucmotCommand.joint][2] * delta +
            //         zucmotCommand.path_info[i - 1].pathq_coefs[zucmotCommand.joint][1] * delta * delta + zucmotCommand.path_info[i - 1].pathq_coefs[zucmotCommand.joint][0] * delta * delta * delta;
            // j2 = zucmotCommand.path_info[i].pathq_coefs[zucmotCommand.joint][3] + zucmotCommand.path_info[i].pathq_coefs[zucmotCommand.joint][2] * delta +
            //         zucmotCommand.path_info[i].pathq_coefs[zucmotCommand.joint][1] * delta * delta + zucmotCommand.path_info[i].pathq_coefs[zucmotCommand.joint][0] * delta * delta * delta;

            // j3 = zucmotCommand.path_info[i - 1].pathq_coefs[lasttc_jointNum][3] + zucmotCommand.path_info[i - 1].pathq_coefs[lasttc_jointNum][2] * delta +
            //         zucmotCommand.path_info[i - 1].pathq_coefs[lasttc_jointNum][1] * delta * delta + zucmotCommand.path_info[i - 1].pathq_coefs[lasttc_jointNum][0] * delta * delta * delta;
            // j4 = zucmotCommand.path_info[i].pathq_coefs[lasttc_jointNum][3] + zucmotCommand.path_info[i].pathq_coefs[lasttc_jointNum][2] * delta +
            //         zucmotCommand.path_info[i].pathq_coefs[lasttc_jointNum][1] * delta * delta + zucmotCommand.path_info[i].pathq_coefs[lasttc_jointNum][0] * delta * delta * delta;

            // if(fabs(j3 - j4) > fabs(j1 - j2)){
            //     delta_tar = fabs(j3 - j4);
            // } else {
            //     delta_tar = fabs(j1 - j2);
            // }
            delta_tar = 0.0;
            for (int jnum = 0; jnum < zucStatus->motion.robot_joint_num; jnum++)
            {
                delta_jpos = fabs(jpos1[jnum] - jpos2[jnum]);
                if (delta_tar < delta_jpos)
                {
                    delta_tar = delta_jpos;
                }
            }

            // if (trajjointmov.ini_maxvel > V_FUZZ)
            // {  //min_time的计算需改进，主要是转接和调倍率的影响
            zucmotCommand.path_info[i].min_time = delta_tar / zucmotCommand.trajjointmov.ini_maxvel;
            // }
            // else
            // {
            //     zucmotCommand.path_info[i].min_time = 1e8;
            // }
            // if (tcp_vel_limit_toppra < V_FUZZ)
            // {
            //     zucmotCommand.path_info[i].min_time = 1e8;
            // }
            // else
            // {
            min_time_cart = delta_tar_cart / tcp_vel_limit_toppra;
            if (min_time_cart > zucmotCommand.path_info[i].min_time)
            {
                zucmotCommand.path_info[i].min_time = min_time_cart;
            }
            // }
        }
        else
        {
            delta_tar = delta_tar_cart;
            // double cart_vel = 0.0;
            if (zucmotCommand.motion_type == TC_MOVC)
            {
                zucmotCommand.path_info[i].min_time = delta_tar / zucmotCommand.trajmovc.ini_maxvel;
                // if (cart_vel > tcp_vel_limit_toppra)
                // {
                //     cart_vel = tcp_vel_limit_toppra;
                // }
                // if (min_time_cart > zucmotCommand.path_info[i].min_time)
                // {
                //     zucmotCommand.path_info[i].min_time = min_time_cart;
                // }
                // else
                // {
                //     zucmotCommand.path_info[i].min_time = 1e8;
                // }
            }
            else if (zucmotCommand.motion_type == TC_LINEAR)
            {
                if (zucmotCommand.trajlinearmov.using_abc_vel)
                {
                    min_time_cart =
                        delta_tar / zucmotCommand.trajlinearmov.ini_maxvel;  //以姿态做规划主轴时，这里暂时使用轴的最大速度，后续需要改善的话再进行修改
                    if (min_time_cart > zucmotCommand.path_info[i].min_time)
                    {
                        zucmotCommand.path_info[i].min_time = min_time_cart;
                    }
                }
                else
                {
                    zucmotCommand.path_info[i].min_time = delta_tar / zucmotCommand.trajlinearmov.ini_maxvel;
                    // if (cart_vel > tcp_vel_limit_toppra)
                    // {
                    //     cart_vel = tcp_vel_limit_toppra;
                    // }
                    // if (min_time_cart > zucmotCommand.path_info[i].min_time)
                    // {
                    //     zucmotCommand.path_info[i].min_time = min_time_cart;
                    // }
                    // else
                    // {
                    //     zucmotCommand.path_info[i].min_time = 1e8;
                    // }
                }
            }
        }

        for (j = 6; j < 12; j++)
        {
            k = j - 6;
            // zucmotCommand.path_info[i].jacc_a[j] = zucmotCommand.path_info[i - 1].jacc_a[k] - 2 * delta * zucmotCommand.path_info[i - 1].jacc_b[k];
            // zucmotCommand.path_info[i].jacc_b[j] = zucmotCommand.path_info[i - 1].jacc_b[k];

            zucmotCommand.path_info[i].jtor_a[j] = zucmotCommand.path_info[i - 1].jtor_a[k] - 2 * delta * zucmotCommand.path_info[i - 1].jtor_b[k];
            zucmotCommand.path_info[i].jtor_b[j] = zucmotCommand.path_info[i - 1].jtor_b[k];
            zucmotCommand.path_info[i].jtor_c[j] = zucmotCommand.path_info[i - 1].jtor_c[k];
            zucmotCommand.path_info[i].jtor_d[j] = zucmotCommand.path_info[i - 1].jtor_d[k];
            // rtapi_print("[task]: %d, %dth jtor_param = %f %f %f %f\n", i, j, zucmotCommand.path_info[i].jtor_a[j], zucmotCommand.path_info[i].jtor_b[j], zucmotCommand.path_info[i].jtor_c[j],
            //         zucmotCommand.path_info[i].jtor_d[j]);
        }
        // rtapi_print("interpolate: \n");
        // rtapi_print("jtor_a = %f %f %f %f %f %f, %f %f %f %f %f %f\n", zucmotCommand.path_info[i].jtor_a[0], zucmotCommand.path_info[i].jtor_a[1], zucmotCommand.path_info[i].jtor_a[2],
        //         zucmotCommand.path_info[i].jtor_a[3], zucmotCommand.path_info[i].jtor_a[4], zucmotCommand.path_info[i].jtor_a[5], zucmotCommand.path_info[i].jtor_a[6],
        //         zucmotCommand.path_info[i].jtor_a[7], zucmotCommand.path_info[i].jtor_a[8],
        //         zucmotCommand.path_info[i].jtor_a[9], zucmotCommand.path_info[i].jtor_a[10], zucmotCommand.path_info[i].jtor_a[11]);
        // rtapi_print("jtor_b = %f %f %f %f %f %f, %f %f %f %f %f %f\n", zucmotCommand.path_info[i].jtor_b[0], zucmotCommand.path_info[i].jtor_b[1], zucmotCommand.path_info[i].jtor_b[2],
        //         zucmotCommand.path_info[i].jtor_b[3], zucmotCommand.path_info[i].jtor_b[4], zucmotCommand.path_info[i].jtor_b[5], zucmotCommand.path_info[i].jtor_b[6],
        //         zucmotCommand.path_info[i].jtor_b[7], zucmotCommand.path_info[i].jtor_b[8],
        //         zucmotCommand.path_info[i].jtor_b[9], zucmotCommand.path_info[i].jtor_b[10], zucmotCommand.path_info[i].jtor_b[11]);
    }
    return 0;
}

int compute_pathvel_by_reqvel(const int jpos_len)  //不在这里计算速度带来的约束，在后面实时轨迹规划的时候再计算，以实现调倍率
{
    return 0;
}

double get_min(double a, double b)
{
    double res;
    res = a;
    if (res > b)
    {
        res = b;
    }
    return res;
}

double get_max(double a, double b)
{
    double res;
    res = a;
    if (res < b)
    {
        res = b;
    }
    return res;
}

void init_DYN()  // for test dynamics
{
    // rtapi_print(" -------------------------------- toppra_test init_DYN -------------------------------- \n");
    // double toolParameters[4] = {0.0, 0.0, 0.0, 0.0};
    // double dynP[60] = {6.7548, -1.2386, 0.1100, 0.6179, -0.0592, 6.2814, -0.2237, 0.0094, -0.0446, 0.2105,
    //               0.5324,  0.0169,-0.0166,-0.0386,  0.0175, 0.0341,  0.0081, 0.0057,  0.0010,-0.0043,
    //               0.0168,  0.0010, 0.0009, 0.0059,  0.0056, 0.0012, -0.0166, 1.4872,  3.5305,-0.0513,
    //               1.4537,  0.0402,-0.0038,-0.2254, -0.0032, 0.0505,  0.0029,-0.0013,  5.0452, 0.4753,
    //               0.4562,  0.4765,12.9006,15.1560, 10.1800, 1.9325,  1.5436, 2.1067, 14.9110, 11.9002,
    //               10.3168, 2.0228, 1.9146, 2.1963, -0.0215, 0.1246, -0.1482, 0.0015, -0.0001, -0.0060};
    // double base_offset_set[3] = {0.0, 0.0, 0.0};
    // double a[6] = {0.000000, 0.000000, 0.360381, 0.304033, 0.000000, 0.000000};
    // double d[6] = {0.120026, 0.000000, 0.000000, -0.115393, 0.113400, 0.106423};
    // double alpha[6] = {0.0, PM_PI_2, 0.0, 0.0, PM_PI_2, -PM_PI_2};
    // DynPara_init_toppra(toolParameters, base_offset_set, dynP, a, d, alpha);
}

int zucMotionSetRunMode(int robot_id, int mode, int serial_num)
{
    if (mode == RUN_MODE_SIM_ON_REAL || mode == RUN_MODE_SIM_WITHOUT_CAN)
    {
        zucStatus->currSimMode = 1;
    }
    else
    {
        zucStatus->currSimMode = 0;
    }

    auto cmd = std::make_shared<CmdSetRunMode>(robot_id, zucStatus->currSimMode, serial_num);
    int ret = mot::realtime_cmd_call(cmd);

    if (task_init_finish_flag)
    {
        usrmotReadZucmotStatus(&zucmotStatus);
        iniChecksumPersistence(zucmotStatus.safetyCRC, CRC_FILE);
    }
    return ret;
}

int zucTrajRigidTap(ZucPose pos, double vel, double ini_maxvel, double acc)
{
#ifdef ISNAN_TRAP
    if (std::isnan(pos.tran.x) || std::isnan(pos.tran.y) || std::isnan(pos.tran.z))
    {
        printf("std::isnan error in zucTrajRigidTap()\n");
        return 0;  // ignore it for now, just don't send it
    }
#endif

    int id = TrajConfig.MotionId;
    zuc_traj_rigid_tap_t trajrigidtap = {};
    trajrigidtap.pos.tran = pos.tran;
    trajrigidtap.vel = vel;
    trajrigidtap.ini_maxvel = ini_maxvel;
    trajrigidtap.acc = acc;

    auto cmd = std::make_shared<CmdRigidTap>(0, id, trajrigidtap);
    return mot::realtime_cmd_call(cmd);
}

static int last_id = 0;
static int last_id_printed = 0;
static int last_status = 0;
static double last_id_time;

int zucIniServoUpgrade()
{
    servo_upgrade_id = shmget(JK_BL_KEY, sizeof(_JK_BL), IPC_CREAT | 0666);
    if (servo_upgrade_id == -1)
    {
        printf("failed to get share memory for servo_upgrade_id\n");
        //perror("shmget err");
        //return -1;
    }
    servo_upgrade = (_JK_BL*)shmat(servo_upgrade_id, NULL, 0);
    if (servo_upgrade == (void*)-1)
    {
        printf("failed to map servo_upgrade share memory\n");
        //perror("shmget err ");
        //return -1;
    }
    servo_upgrade->TotalProgress = -1;  //FIXME：这里设置-1，但因为是无符号的所以在python里是65535
    servo_upgrade->ErrorCode = 0;
    return 0;
}

int zucExtIOSetReconnectParams(int reConnectInterval, int reConnectMaxTimes, int saveFlag)
{
    if (reConnectInterval < 0 || reConnectMaxTimes < 0)
        return -1;

    if (saveFlag)
    {
        ExtioConfig config;
        config.iniSaveExtioReconnectParams(ZUC_USER_CONFIG_FILE(usersettings.ini), reConnectInterval, reConnectMaxTimes);
    }

    zucmotCommand.command = ZUCMOT_SETRECONNECT_EXTIO;
    zucmotCommand.extIOsetup.reConnectInterval = reConnectInterval;
    zucmotCommand.extIOsetup.reConnectMaxTimes = reConnectMaxTimes;

    return usrmotWriteZucmotCommand(&zucmotCommand);
}

int zucExtIOSetMode(int mode)
{
    zucmotCommand.command = ZUCMOT_SETMODE_EXTIO;
    // zucmotCommand.mode = mode;
    zucmotCommand.extIOsetup.mode = mode;

    return usrmotWriteZucmotCommand(&zucmotCommand);
}

int zucExtIOUpdate(ZUC_EXTIO_STAT* stat)
{
    stat->extIOStat = zucmotStatus.extio_stat;
    return 0;
}
int zucExtIOSetup(int extIOModCnt, ExtIOSetup* extIOSetups, int saveFlag)  //
{
    // Manage the IO map, setup --> IO map, then send to motion module
    // int numInRange = 0;
    int totalPinNum[4] = {0};  //di, do, ai, ao
    for (int i = 0; i < extIOModCnt; ++i)
    {
        totalPinNum[0] += extIOSetups[i].diSetup.pinNum;
        totalPinNum[1] += extIOSetups[i].doSetup.pinNum;
        totalPinNum[2] += extIOSetups[i].aiSetup.pinNum;
        totalPinNum[3] += extIOSetups[i].aoSetup.pinNum;
    }

    // ensure the total pin number
    if (totalPinNum[0] > MAX_EXTIO_DI_NUM || totalPinNum[1] > MAX_EXTIO_DO_NUM || totalPinNum[2] > MAX_EXTIO_AI_NUM || totalPinNum[3] > MAX_EXTIO_AO_NUM)
    {
        zucOperatorError(EXTIO_ERR_INIT_RTU_CONNECTION, " extDI/DO num exeed 64 or AI/AO num exceed 32  ");
        // rcs_print("totalPinNum = %d  %d   %d  %d    \n", totalPinNum[0],totalPinNum[1],totalPinNum[2],totalPinNum[3]);
        return -1;
    }

    zucmotCommand.command = ZUCMOT_SETUP_EXTIO;
    // zucmotCommand.extIOModCnt = extIOModCnt;
    zucmotCommand.extIOsetup.extIOModCnt = extIOModCnt;
    zucStatus->io.extio.extIOModCnt = extIOModCnt;
    // rcs_print("zucStatus->io.extio.extIOModCnt = %d\n", extIOModCnt);

    int availIdx[4] = {0, 0, 0, 0};  //di, do, ai, ao
    for (int i = 0; i < extIOModCnt; ++i)
    {
        // zucmotCommand.extIOModConfig[i].extIOSetup = extIOSetups[i];
        // zucmotCommand.extIOModConfig[i].extIOMap.diStartIdx = availIdx[0];
        // availIdx[0] += extIOSetups[i].diSetup.pinNum;
        // zucmotCommand.extIOModConfig[i].extIOMap.doStartIdx = availIdx[1];
        // availIdx[1] += extIOSetups[i].doSetup.pinNum;
        // zucmotCommand.extIOModConfig[i].extIOMap.aiStartIdx = availIdx[2];
        // availIdx[2] += extIOSetups[i].aiSetup.pinNum;
        // zucmotCommand.extIOModConfig[i].extIOMap.aoStartIdx = availIdx[3];
        // availIdx[3] += extIOSetups[i].aoSetup.pinNum;
        zucmotCommand.extIOsetup.extIOModConfig[i].extIOSetup = extIOSetups[i];
        zucmotCommand.extIOsetup.extIOModConfig[i].extIOMap.diStartIdx = availIdx[0];
        availIdx[0] += extIOSetups[i].diSetup.pinNum;
        zucmotCommand.extIOsetup.extIOModConfig[i].extIOMap.doStartIdx = availIdx[1];
        availIdx[1] += extIOSetups[i].doSetup.pinNum;
        zucmotCommand.extIOsetup.extIOModConfig[i].extIOMap.aiStartIdx = availIdx[2];
        availIdx[2] += extIOSetups[i].aiSetup.pinNum;
        zucmotCommand.extIOsetup.extIOModConfig[i].extIOMap.aoStartIdx = availIdx[3];
        availIdx[3] += extIOSetups[i].aoSetup.pinNum;

        zucStatus->io.extio.extIOConfig[i].extIOSetup = extIOSetups[i];
        // zucStatus->io.extio.extIOConfig[i].extIOMap   = zucmotCommand.extIOModConfig[i].extIOMap;
        zucStatus->io.extio.extIOConfig[i].extIOMap = zucmotCommand.extIOsetup.extIOModConfig[i].extIOMap;
    }

    /*save the extio setup*/
    if (saveFlag)
    {
        xmlSaveExtIOSetup(extIOModCnt, extIOSetups);
    }

    return usrmotWriteZucmotCommand(&zucmotCommand);
}

int zucSetMaintenanceMode(int maintenance)
{
    zucStatus->motion.maintenance_status = maintenance;
    auto cmd = std::make_shared<CmdSetMaintenanceMode>(0, maintenance);
    return mot::realtime_cmd_call(cmd);
}

//
int zucModbusSetupSlave(int commType, CommInfo mbComm)
{
    if (commType == 0 || commType == 1)
    {
        zucStatus->mbSlaveCommType = commType;
        zucStatus->mbSlaveCommInfo = mbComm;

        rcs_print("mbSlave type is %d\n", zucStatus->mbSlaveCommType);

        iniSaveSlaveConfig(commType, mbComm, zuc_inifile);
        return 0;
    }
    else
    {
        rcs_print("invalid modbus slave type\n");
        return -1;
    }
}

int zucSetTrajSmooth(int trqLimitActive, int firFltActive)
{
    auto cmd = std::make_shared<CmdSetTrajsmooth>(0, trqLimitActive, firFltActive);
    return mot::realtime_cmd_call(cmd);
}

int zucTorqSensorUpdate(ZUC_SENSOR_STAT* stat)
{
    stat->sensorStatus = zucmotStatus.sensor_stat;
    stat->sensorStatus.realTorque[0] = zucmotStatus.actualTorque[0];
    stat->sensorStatus.realTorque[1] = zucmotStatus.actualTorque[1];
    stat->sensorStatus.realTorque[2] = zucmotStatus.actualTorque[2];
    stat->sensorStatus.realTorque[3] = zucmotStatus.actualTorque[3];
    stat->sensorStatus.realTorque[4] = zucmotStatus.actualTorque[4];
    stat->sensorStatus.realTorque[5] = zucmotStatus.actualTorque[5];
    return 0;
}

int zucSetCollisionGmRecordEnable(int gm_record_enable)
{
    // zucStatus->motion.gm_record_enable = gm_record_enable
    auto cmd = std::make_shared<CmdSetCollisionGmRecordEnable>(0, gm_record_enable);
    return mot::realtime_cmd_call(cmd);
}

int zucTioMBDelSignal(const char* name)
{
    if (name == NULL || strlen(name) > 128)
        return -1;

    int res = 0;
    zucmotCommand.command = ZUCMOT_MBTIO_DEL_SIGNAL;
    strcpy(zucmotCommand.tioplsDelSig.sigName, name);
    // update the configuration file
    if ((res = usrmotWriteZucmotCommand(&zucmotCommand)) == 0)
    {
        usrmotReadZucmotStatus(&zucmotStatus);
        xmlSaveTIOSignals(MAX_TIO_SIG_NUM, zucmotStatus.gripperState);
    }
    return res;
}

int zucTioMBUpdateSignal(const char* name, float freq)
{
    if (name == NULL || strlen(name) > 128)
        return -1;

    zucmotCommand.command = ZUCMOT_MBTIO_UPDATE_SIGNAL;
    strcpy(zucmotCommand.tioplsUpdateSig.sigName, name);
    zucmotCommand.tioplsUpdateSig.freq = freq;

    for (int i = 0; i < MAX_TIO_SIG_NUM; ++i)
    {
        if (strcmp(zucmotStatus.gripperState[i].sigName, name) == 0)
        {
            //zucmotStatus.gripperState[i].freq = freq;
            gripperFreq[i] = freq;
            const char* filePath = ZUC_USER_CONFIG_DIR "/tiosignals.xml ";
            xmlUpdateTIOSignals(filePath, name, "Frequency", (int)freq);
        }
    }
    return usrmotWriteZucmotCommand(&zucmotCommand);
}

int zucTioMBSendCommand(int chnId, const unsigned char* cmd, int len)
{
    if (cmd == NULL || len >= 32)
        return -1;

    zucmotCommand.command = ZUCMOT_MBTIO_SEND_COMMAND;
    zucmotCommand.tioplsSendCmd.chnId = chnId;
    zucmotCommand.tioplsSendCmd.len = len;
    memcpy(zucmotCommand.tioplsSendCmd.cmd, cmd, len);
    return usrmotWriteZucmotCommand(&zucmotCommand);
}

int zucMotionInit()
{
    int r1 = mot::al::MotionProxy::instance().motion_init();  // 初始化所有机器人

    // 初始化共享内存，motion层;
    if (0 != usrmotInit("zuc_task"))
    {
        return -1;
    }

    for (size_t i = 0; i < mot::al::MotionProxy::instance().robot_cnt(); i++)
    {
        auto rob = mot::al::MotionProxy::instance().robot(i);
        if (!rob.get())
        {
            return -1;
        }
        // 处理仿真
        if (zuc_sim == ZUC_SIM_ENA_SOFT)
        {
            zucMotionSetRunMode(i, RUN_MODE_SIM_WITHOUT_CAN, 0);  // simulation with out CAN connection
        }
        else if (zuc_sim == ZUC_SIM_ENA_ON_DEV)
        {
            zucMotionSetRunMode(i, RUN_MODE_SIM_ON_REAL, 0);  // simulation on real device
        }
        else
        {
            zucMotionSetRunMode(i, RUN_MODE_REAL, 0);  // real device
        }
    }

    // for (size_t i = 0; i < mot::al::MotionProxy::instance().robot_cnt(); i++)
    // {
    //     auto rob = mot::al::MotionProxy::instance().robot(i); if (!rob.get()) {return -1;}
    //     rob->zucTrajInit();
    // }

    // int r1, r2, r3, r4;
    // int joint, axis;

    // // rcs_print("zucMotionInit called\n");
    // // init_DYN();

    // rcs_print("r2 = %d\n", r2);

    // rcs_print("r3 = %d\n", r3);

    // if (r1 == 0 && r2 == 0 && r3 == 0)
    {
        zucmotion_initialized = 1;
    }
    // initSafetyCRC(CRC_FILE);
    // zucIniServoUpgrade();

    return (r1 == 0) ? 0 : -1;
}

int zucMotionSetDebug(int debug)
{
    auto cmd = std::make_shared<CmdSetDebug>(0, debug);
    return mot::realtime_cmd_call(cmd);
}

int zucMotionExitProtect(int type)
{
    auto cmd = std::make_shared<CmdExitProtect>(0, type);
    return mot::realtime_cmd_call(cmd);
}

int zucMotionPercentageMode(int level)
{
    auto cmd = std::make_shared<CmdPercentageMode>(0, level);
    return mot::realtime_cmd_call(cmd);
}

int zucMotionPercentageModeLevelValue(double percentage_mode_level1_val, double percentage_mode_level2_val)
{
    // 倍率范围 0~1
    if (percentage_mode_level1_val < 0)
        percentage_mode_level1_val = 0;
    if (percentage_mode_level1_val > 1)
        percentage_mode_level1_val = 1;
    if (percentage_mode_level2_val < 0)
        percentage_mode_level2_val = 0;
    if (percentage_mode_level2_val > 1)
        percentage_mode_level2_val = 1;

    if (percentage_mode_level2_val > percentage_mode_level1_val)  // 二级倍率 不高于一级倍率
    {
        printf("zucMotionPercentageModeLevelValue(%lf,  %lf): paramaters check [ FAILED ] \n", percentage_mode_level1_val, percentage_mode_level2_val);
        return 0;
        //percentage_mode_level2_val = percentage_mode_level1_val;
    }

    iniSavePercentageModeValue(percentage_mode_level1_val, percentage_mode_level2_val, zuc_inifile);

    auto cmd = std::make_shared<CmdSetPercentageModeLevelVal>(0, percentage_mode_level1_val, percentage_mode_level2_val);
    return mot::realtime_cmd_call(cmd);
}

int zucMotReduceMode(int reduceMode)
{
    auto cmd = std::make_shared<CmdReduceMode>(0, reduceMode);
    return mot::realtime_cmd_call(cmd);
}

int zucSetNetExceptionHandle(float timeLimit, int motType)
{
    zucStatus->netExceptionHandle.type = motType;
    zucStatus->netExceptionHandle.timeLimit = timeLimit;
    iniSetNetExceptionHandle(timeLimit, motType);
    return 0;
}

int zucMotionKineInverse(int robid, const struct ZucPose* world, double* joint)
{
    KINEMATICS_FORWARD_FLAGS curr_fflags = 0;
    KINEMATICS_INVERSE_FLAGS curr_iflags = 0;

    usrOpeErrCode = -1;
    for (int i = 0; i < zucStatus->motion.robot_joint_num; ++i) { zucStatus->ext_motion[0].utilJointPos[i] = 0.0; }

    // rcs_print("zucMotionKineInverse: currJoint: %f, %f, %f, %f, %f, %f",
    // joint[0], joint[1], joint[2], joint[3],joint[4],joint[5]);

    // getInvKineFlag(joint, &curr_iflags);
    // update_inverse_flag(joint, &curr_fflags, &curr_iflags);
    usrOpeErrCode =
        kinematicsInverse(&zucmotConfig.tool_offset, world, joint, &curr_iflags, &curr_fflags, &zucmotConfig.base_offset, &zucmotConfig.user_offset, robid);
    for (int i = 0; i < zucStatus->motion.robot_joint_num; i++) { zucStatus->ext_motion[0].utilJointPos[i] = joint[i]; }
    zucStatus->usrOpeErrCode = usrOpeErrCode;
    return usrOpeErrCode;
}

// int zucMotionAPP(unsigned char appver){
//     zucmotCommand.motsetADout.appver = appver;
//     return usrmotWriteZucmotCommand(&zucmotCommand);
// }

int zucMotionKineForward(int robid, const double* joint)
{
    unsigned long curr_iflags = 0, curr_fflags = 0;
    usrOpeErrCode = -1;

    ZERO_ZUC_POSE(zucStatus->ext_motion[0].utilCartPos);
    usrOpeErrCode = kinematicsForward(&zucmotConfig.tool_offset,
                                      joint,
                                      &zucStatus->ext_motion[0].utilCartPos,
                                      &curr_fflags,
                                      &curr_iflags,
                                      &zucmotConfig.base_offset,
                                      &zucmotConfig.user_offset,
                                      robid);
    zucStatus->usrOpeErrCode = usrOpeErrCode;
    return usrOpeErrCode;
}

/*! \function zucMotionSetAout()

    This function sends a ZUCMOT_SET_AOUT message to the motion controller.
    That one plans a AOUT command when motion starts or right now.

    @parameter	index	which output gets modified
    @parameter	now	wheather change is imediate or synched with motion
    @parameter	start	value set at start of motion
    @parameter	end	value set at end of motion
*/
int zucMotionSetAout(unsigned char ioType, unsigned char index, double start, double end, unsigned char now)
{
    if (zucmotStatus.protective_stop == 1 && zucStatus->task.interpState == ZUC_TASK_INTERP_IDLE)
    {
        rcs_print("Cannot operate IO when in protective_stop status\n");
        return -1;
    }

    zucmotCommand.command = ZUCMOT_SET_AOUT;
    // zucmotCommand.ioType = ioType;
    // zucmotCommand.now = now;
    // zucmotCommand.out = index;
    zucmotCommand.motsetADout.ioType = ioType;
    zucmotCommand.motsetADout.now = now;
    zucmotCommand.motsetADout.out = index;
    /*! \todo FIXME-- if this works, set up some dedicated cmd fields instead of
     borrowing these */
    // zucmotCommand.minLimit = start;
    // zucmotCommand.maxLimit = end;
    zucmotCommand.motsetADout.minLimit = start;
    zucmotCommand.motsetADout.maxLimit = end;

    return usrmotWriteZucmotCommand(&zucmotCommand);
}

/*! \function zucMotionSetDout()
    
    This function sends a ZUCMOT_SET_DOUT message to the motion controller.
    That one plans a DOUT command when motion starts or right now.

    @parameter	index	which output gets modified
    @parameter	now	wheather change is imediate or synched with motion
    @parameter	start	value set at start of motion
    @parameter	end	value set at end of motion
*/

//
int zucMotionSetDout(unsigned char ioType, int index, unsigned char start, unsigned char end, unsigned char now)
{
    if (zucmotStatus.protective_stop == 1 && zucStatus->task.interpState == ZUC_TASK_INTERP_IDLE)
    {
        rcs_print("Cannot IO operate when in protective_stop status\n");
        return -1;
    }

    zucmotCommand.command = ZUCMOT_SET_DOUT;

    zucmotCommand.motsetADout.ioType = ioType;
    zucmotCommand.motsetADout.now = now;
    zucmotCommand.motsetADout.out = index;
    zucmotCommand.motsetADout.start = start;
    zucmotCommand.motsetADout.end = end;

    return usrmotWriteZucmotCommand(&zucmotCommand);
}

int zucSetCapture(int captId, IOTrigger trigger, int stateToCapt)
{
    if (captId < 0 || captId >= 8)
        return -1;
    zucmotCommand.command = ZUCMOT_SERVO_SET_CAPTURE;
    zucmotCommand.setcapture.captId = captId;
    zucmotCommand.setcapture.trigger = trigger;
    zucmotCommand.setcapture.stateToCapt = stateToCapt;
    return usrmotWriteZucmotCommand(&zucmotCommand);
}

int zucClearCapture(int captId)
{
    if (captId < 0 || captId >= 8)
        return -1;
    zucmotCommand.command = ZUCMOT_SERVO_CLEAR_CAPTURE;
    zucmotCommand.clearcapture.captId = captId;
    return usrmotWriteZucmotCommand(&zucmotCommand);
}

int zucShutDown(void)
{
    auto cmd = std::make_shared<CmdShutDown>();
    return mot::realtime_cmd_call(cmd);
}

int zucServoUpGrade(void)
{
    servo_upgrade->TotalProgress = 0;  //先断开再置0
    auto cmd = std::make_shared<CmdServoUpgrade>(0);
    return mot::realtime_cmd_call(cmd);
}

static int resetPayloadIdentifyStat()
{
    auto cmd = std::make_shared<CmdResetIdentifyStat>(0);
    return mot::realtime_cmd_call(cmd);
}

int zucSafetyCrc(unsigned safetycrc)
{
    auto cmd = std::make_shared<CmdInitSafetyCrc>(0, safetycrc);
    int ret = mot::realtime_cmd_call(cmd);
    if (task_init_finish_flag)
    {
        usrmotReadZucmotStatus(&zucmotStatus);
        iniChecksumPersistence(zucmotStatus.safetyCRC, CRC_FILE);
    }

    return ret;
}

int zucSetupArcBlends(int arcBlendEnable,
                      int arcBlendFallbackEnable,
                      int arcBlendOptDepth,
                      int arcBlendGapCycles,
                      double arcBlendRampFreq,
                      double arcBlendTangentKinkRatio)
{
    zuc_setup_arc_blends_t setuparcblends = {};

    setuparcblends.arcBlendEnable = arcBlendEnable;
    setuparcblends.arcBlendFallbackEnable = arcBlendFallbackEnable;
    setuparcblends.arcBlendOptDepth = arcBlendOptDepth;
    setuparcblends.arcBlendGapCycles = arcBlendGapCycles;
    setuparcblends.arcBlendRampFreq = arcBlendRampFreq;
    setuparcblends.arcBlendTangentKinkRatio = arcBlendTangentKinkRatio;

    auto cmd = std::make_shared<CmdSetupArcBlends>(0, setuparcblends);
    return mot::realtime_cmd_call(cmd);
}

int zucSetMaxFeedOverride(double maxFeedScale)
{
    auto cmd = std::make_shared<CmdSetMaxFeedOverride>(0, maxFeedScale);
    return mot::realtime_cmd_call(cmd);
}

int zucAdapt2Robot(unsigned int robotSerialNum)
{
    int model;
    DHParam dhParam = {};
    RobotModelData robotModelData = {};

    zucStatus->motion.robot_joint_num = 6;

    // get robot model from serial number
    zucGetModelFromSerialNum(robotSerialNum, &model);
    if (iniLoadRobotModelData(model, robotSerialNum, &robotModelData) != 0)
    {
        zucOperatorError(UNSUPPORTED_ROBOT_MODEL, "model of connected robot is now unsupported");
        return -1;
    }

    zucStatus->motion.robot_model_data = robotModelData;
    // printf(">>>>>>>>>>>>>>>>>>%d,  %d,  %d  \n",robotModelData.model.productNumber, robotModelData.model.productVersion, robotModelData.model.robotSeries);

    zucSetDynamicsIdentifyTraj(robotModelData.dynamics_identify_traj_para);

    // ZucIniFile trajIniFile(ZucIniFile::ERR_TAG_NOT_FOUND | ZucIniFile::ERR_SECTION_NOT_FOUND | ZucIniFile::ERR_CONVERSION);
    Util::IniParser ini_file(zuc_inifile);
    if (iniLoadHistoryDHParam(&ini_file, robotSerialNum, &dhParam) != 0)
    {
        dhParam = robotModelData.nominalDH;
    }

    // zucsetJointOffsetAndDir(robotModelData.joint_offset, robotModelData.joint_dir);

    if (zuc_cab_type >= 2)
    {
        zucInitSafetyRobotPowerLimitSettings(zuc_inifile);
    }
    zucInitRobotSafetyLimitSettings(zuc_inifile);
    zucInitRobotReduceSafetyLimitSettings(zuc_inifile);
    // 设置机器人安全参数
    if (0 != zucSetFunctionSaftyPara(
                 robotModelData.robotPowerLimit, robotModelData.robotMomentumLimit, robotModelData.robotEndforceLimit, robotModelData.robotVelocityLimit))
    {
        return -1;
    }

    for (int i = 0; i < zucStatus->motion.robot_joint_num; ++i) { JointConfig[i].HMaxVel = robotModelData.maxJointVel[i]; }

    zucStatus->motion.robot_serial_num = robotSerialNum;
    return 0;
}

int updateRobotSafety(int model)
{
    try
    {
        boost::property_tree::ptree m_pt, tag_setting;
        std::string modelType = deterRobotModel(model);
        Util::IniParser ini_file(modelType.c_str());
        //trajInifile.Open(modelType.c_str());
        std::string section[4] = {"NOLIMITSAFETYSETTING", "LOOSESAFETYSETTING", "MEDIUMSAFETYSETTING", "STRICTSAFETYSETTING"};
        for (int i = 0; i < 4; ++i)
        {
            double momentum_limit, tcp_vel_limit, robot_power_limit, stopping_dist, stopping_time;
            std::string node;
            node = section[i] + ".MOMENTUM_LIMIT";
            ini_file.get(node, momentum_limit, 70.0, 0, __DBL_MAX__);  //默认使用710 LOOSESAFETYSETTING参数
            node = section[i] + ".TCP_VEL_LIMIT";
            ini_file.get(node, tcp_vel_limit, 1620.0, 0, __DBL_MAX__);  //默认使用710 LOOSESAFETYSETTING参数
            node = section[i] + ".ROBOT_POWER_LIMIT";
            ini_file.get(node, robot_power_limit, 1500.0, 0, __DBL_MAX__);  //默认使用710 LOOSESAFETYSETTING参数
            node = section[i] + ".STOPPING_DIST";
            ini_file.get(node, stopping_dist, 500.0, 0, __DBL_MAX__);  //默认使用710 LOOSESAFETYSETTING参数
            node = section[i] + ".STOPPING_TIME";
            ini_file.get(node, stopping_time, 0.4, 0, __DBL_MAX__);  //默认使用710 LOOSESAFETYSETTING参数
            // trajInifile.Find(&x, "MOMENTUM_LIMIT", section[i].c_str());
            // trajInifile.Find(&y, "TCP_VEL_LIMIT", section[i].c_str());
            // trajInifile.Find(&z, "ROBOT_POWER_LIMIT", section[i].c_str());
            // trajInifile.Find(&stopping_dist, "STOPPING_DIST", section[i].c_str());
            // trajInifile.Find(&stopping_time, "STOPPING_TIME", section[i].c_str());

            zucStatus->robotSafe[i + 2].momentumLimit = momentum_limit;
            zucStatus->robotSafe[i + 2].tcpVelLimit = tcp_vel_limit;
            zucStatus->robotSafe[i + 2].robotPowerLimit = robot_power_limit;
            zucStatus->robotSafe[i + 2].stoppingDistance = stopping_dist;
            zucStatus->robotSafe[i + 2].stoppingTime = stopping_time;
        }

        std::string filename = ZUC_USER_CONFIG_DIR "/usersettings.ini";
        read_ini(filename, m_pt);

        if (m_pt.get_child_optional("ROBOTSAFETYSETTING"))
        {
            tag_setting = m_pt.get_child("ROBOTSAFETYSETTING");
            zucStatus->robotSafe[0].momentumLimit = tag_setting.get<double>("MOMENTUM_LIMIT");
            zucStatus->robotSafe[0].tcpVelLimit = tag_setting.get<double>("TCP_VEL_LIMIT");
            zucStatus->robotSafe[0].robotPowerLimit = tag_setting.get<double>("ROBOT_POWER_LIMIT");
            zucStatus->robotSafe[0].stoppingDistance = tag_setting.get<double>("STOPPING_DIST");
            zucStatus->robotSafe[0].stoppingTime = tag_setting.get<double>("STOPPING_TIME");
            zucStatus->robotSafe[0].custome = tag_setting.get<double>("CUSTOM_TYPE");
            zucStatus->robotSafe[0].lastCustome = tag_setting.get<double>("LAST_CUSTOM_TYPE");
            zucStatus->robotSafe[1].momentumLimit = tag_setting.get<double>("OLD_MOMENTUM_LIMIT");
            zucStatus->robotSafe[1].tcpVelLimit = tag_setting.get<double>("OLD_TCP_VEL_LIMIT");
            zucStatus->robotSafe[1].robotPowerLimit = tag_setting.get<double>("OLD_ROBOT_POWER_LIMIT");
            zucStatus->robotSafe[1].stoppingDistance = tag_setting.get<double>("OLD_STOPPING_DIST");
            zucStatus->robotSafe[1].stoppingTime = tag_setting.get<double>("OLD_STOPPING_TIME");

            // 超出范围的停止时间停止距离设置为实际的生效值
            if (zucStatus->robotSafe[0].stoppingDistance > 4000.0)
            {
                zucStatus->robotSafe[0].stoppingDistance = 4000.0;
            }
            else if (zucStatus->robotSafe[0].stoppingDistance < 100.0)
            {
                zucStatus->robotSafe[0].stoppingDistance = 100.0;
            }

            if (zucStatus->robotSafe[1].stoppingDistance > 4000.0)
            {
                zucStatus->robotSafe[1].stoppingDistance = 4000.0;
            }
            else if (zucStatus->robotSafe[1].stoppingDistance < 100.0)
            {
                zucStatus->robotSafe[1].stoppingDistance = 100.0;
            }

            if (zucStatus->robotSafe[0].stoppingTime > 5)
            {
                zucStatus->robotSafe[0].stoppingTime = 5;
            }
            else if (zucStatus->robotSafe[0].stoppingTime < 0.1)
            {
                zucStatus->robotSafe[0].stoppingTime = 0.1;
            }

            if (zucStatus->robotSafe[1].stoppingTime > 5)
            {
                zucStatus->robotSafe[1].stoppingTime = 5;
            }
            else if (zucStatus->robotSafe[1].stoppingTime < 0.1)
            {
                zucStatus->robotSafe[1].stoppingTime = 0.1;
            }
        }
        else
        {
            int lastCustome = zucStatus->robotSafe[0].lastCustome;
            if (zucStatus->robotSafe[0].custome != lastCustome && zucStatus->robotSafe[0].custome != 0)
            {
                lastCustome = zucStatus->robotSafe[0].custome;
            }
            zucStatus->robotSafe[0].momentumLimit = zucStatus->robotSafe[4].momentumLimit;
            zucStatus->robotSafe[0].tcpVelLimit = zucStatus->robotSafe[4].tcpVelLimit;
            zucStatus->robotSafe[0].robotPowerLimit = zucStatus->robotSafe[4].robotPowerLimit;
            zucStatus->robotSafe[0].stoppingDistance = zucStatus->robotSafe[4].stoppingDistance;
            zucStatus->robotSafe[0].stoppingTime = zucStatus->robotSafe[4].stoppingTime;
            iniSaveRobotSafety(model,
                               0,
                               lastCustome,
                               0,
                               zucStatus->robotSafe[0].momentumLimit,
                               zucStatus->robotSafe[0].tcpVelLimit,
                               zucStatus->robotSafe[0].robotPowerLimit,
                               zucStatus->robotSafe[0].stoppingDistance,
                               zucStatus->robotSafe[0].stoppingTime);
        }
    }
    catch (const std::exception& e)
    {
        printf("updateRobotSafety function error:%s\n", e.what());
    }

    return 0;
}

int updateJointSafety(int model, int serial) { return 0; }

int zucSetRobotSafety(ZUC_MOTION_STAT* stat, ZUC_ROBOT_SAFETY robot_safety)
{
    int model;
    // get robot model from serial number
    zucGetModelFromSerialNum(zucStatus->motion.robot_serial_num, &model);

    if (0 != checkRobotSafetyParameters(&robot_safety))
    {
        usrOpeErrCode = -1;
        return -1;
    }

    int lastCustome = zucStatus->robotSafe[0].lastCustome;
    if (zucStatus->robotSafe[0].custome != lastCustome && zucStatus->robotSafe[0].custome != robot_safety.custome)
    {
        lastCustome = zucStatus->robotSafe[0].custome;
    }
    iniSaveRobotSafety(model,
                       robot_safety.custome,
                       lastCustome,
                       robot_safety.paramType,
                       robot_safety.momentumLimit,
                       robot_safety.tcpVelLimit,
                       robot_safety.robotPowerLimit,
                       robot_safety.stoppingDistance,
                       robot_safety.stoppingTime);
    zucGetRobotSafety(stat);
    if (robot_safety.paramType == ZucRobotSafetyType::ROBOT_ALL_PARA || robot_safety.paramType == ZucRobotSafetyType::ROBOT_TCPVEL_PARA)
        zucSetRobotTcpVelLimit(robot_safety.tcpVelLimit);
    if (robot_safety.paramType == ZucRobotSafetyType::ROBOT_ALL_PARA || robot_safety.paramType == ZucRobotSafetyType::ROBOT_MOMENTUM_PARA)
        zucSetRobotMomentumLimit(robot_safety.momentumLimit);
    if (robot_safety.paramType == ZucRobotSafetyType::ROBOT_ALL_PARA || robot_safety.paramType == ZucRobotSafetyType::ROBOT_POWER_PARA)
        zucSetRobotPowerLimit(robot_safety.robotPowerLimit, 1);
    if (robot_safety.paramType == ZucRobotSafetyType::ROBOT_ALL_PARA || robot_safety.paramType == ZucRobotSafetyType::ROBOT_STOPPINGDIST_PARA)
        zucSetRobotStoppingDist(robot_safety.stoppingDistance);
    if (robot_safety.paramType == ZucRobotSafetyType::ROBOT_ALL_PARA || robot_safety.paramType == ZucRobotSafetyType::ROBOT_STOPPINGTIME_PARA)
        zucSetRobotStoppingTime(robot_safety.stoppingTime);
    usrOpeErrCode = 0;
    return 0;
}

int zucSetRobot7dof_invflag(int inv_flag)
{
    try
    {
        auto cmd = std::make_shared<CmdSetRobot7DofInvFlag>(0, inv_flag);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotStoppingTime function error:%s\n", e.what());
        return -1;
    }
}

int zucSetRobotfull_dh_flag(int full_dh_flag)
{
    try
    {
        auto cmd = std::make_shared<CmdSetRobotFullDhFlag>(0, full_dh_flag);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotStoppingTime function error:%s\n", e.what());
        return -1;
    }
}

int zucSetRobot7dof_arm_angle(double arm_angle)
{
    try
    {
        auto cmd = std::make_shared<CmdSetRobot7DofArmAngle>(0, arm_angle);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotStoppingTime function error:%s\n", e.what());
        return -1;
    }
}

int SetRobot7dofinv_inform(int inv_flag, double arm_angle)
{
    zucSetRobot7dof_invflag(inv_flag);
    zucSetRobot7dof_arm_angle(arm_angle);
    return 0;
}

int SetRobotFull_DH_flag(int full_dh_flag)
{
    zucSetRobotfull_dh_flag(full_dh_flag);
    zucStatus->motion.full_dh_com = full_dh_flag;
    return 0;
}

int SetRobot7dof_kine_param_alpha_k(double k, double alpha)
{
    try
    {
        auto cmd = std::make_shared<CmdSetRobot7DofKineParamAlphaK>(0, k, alpha);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotStoppingTime function error:%s\n", e.what());
        return -1;
    }
    return 0;
}

int zucGetRobotSafety(ZUC_MOTION_STAT* stat)
{
    int model;
    // get robot model from serial number
    zucGetModelFromSerialNum(stat->robot_serial_num, &model);
    updateRobotSafety(model);
    return 0;
}

int zucSetCustomizeType(int customize_type)
{
    if (customize_type == 0)
    {
        std::string filename = ZUC_USER_CONFIG_DIR "/usersettings.ini";
        Util::IniParser ini_file(filename.c_str());
        ini_file.set("ROBOTSAFETYSETTING.CUSTOM_TYPE", customize_type);
        //ini_putf("ROBOTSAFETYSETTING", "CUSTOM_TYPE", customize_type, filename.c_str());
        syncFileToDisk(filename.c_str());
    }
    return 0;
}

int zucSetReduceSafety(ZUC_MOTION_STAT* stat, reduce_param_config reduce_param)
{
    int model;
    // get robot model from serial number
    // zucGetModelFromSerialNum(stat->robot_serial_num, &model);
    zucGetModelFromSerialNum(zucStatus->motion.robot_serial_num, &model);
    // Todo
    if (0 != checkReduceSafetyParameters(&reduce_param))
    {
        return -1;
    }
    iniSaveReduceSafety(0, reduce_param);
    zucStatus->reduceSafe.tcp_speed = reduce_param.tcp_vel_reduce;
    zucStatus->reduceSafe.elbow_speed = reduce_param.elbow_vel_limit_reduce;
    zucStatus->reduceSafe.momentum_limit = reduce_param.robot_momentum_limit_reduce;
    zucStatus->reduceSafe.power_limit = reduce_param.robot_poewr_limit_reduce;
    zucStatus->reduceSafe.stopping_dist = reduce_param.stopping_dist_reduce;
    zucStatus->reduceSafe.stopping_time = reduce_param.stopping_time_reduce;
    zucSetRobotElbowVelLimitReduce(reduce_param.elbow_vel_limit_reduce);
    zucSetRobotTcpVelLimitReduce(reduce_param.tcp_vel_reduce);
    zucSetRobotMomentumLimitReduce(reduce_param.robot_momentum_limit_reduce);
    zucSetRobotPowerLimitReduce(reduce_param.robot_poewr_limit_reduce);
    zucSetRobotStoppingDistReduce(reduce_param.stopping_dist_reduce);
    zucSetRobotStoppingTimeReduce(reduce_param.stopping_time_reduce);
    return 0;
}

int zucSetDragTcpSpeedLimitSetting(double dragTcpSpeedLimit)
{
    //伟创力定制的手自动模式功能开启，且在自动模式下，禁止设置安全参数 || 正在运动中则禁止设置安全参数
    if ((zucStatus->operation_mode_option == 1 && zucStatus->operation_mode == 1) || zucStatus->motion.traj.inpos != 1)
    {
        zucOperatorError(NONMANUAL_CANNOT_SET_ROBOT_SAFETY_PARAMETERS,
                         "The safety parameters cannot be configured if the robot is not in manual mode or in manual mode motion.");
        return -1;
    }
    //如果程序执行则禁止设置安全参数
    if (zucStatus->task.interpState == ZUC_TASK_INTERP_READING || zucStatus->task.interpState == ZUC_TASK_INTERP_PAUSED ||
        zucStatus->task.interpState == ZUC_TASK_INTERP_WAITING)
    {
        zucOperatorError(PROGRAM_EXECUTE_CANNOT_SET_ROBOT_SAFETY_PARAMETERS, "The safety parameters cannot be configured if the program is being executed.");
        return -1;
    }
    // if (zucStatus->motion.is_enabled)
    // {
    //     zucOperatorError(ENABLE_CANNOT_SET_ROBOT_SAFETY_PARAMETERS, "The safety parameters cannot be configured if the robot is enable.");
    //     return -1;
    // }
    if ((dragTcpSpeedLimit - 50.0) < -0.0001)
    {
        dragTcpSpeedLimit = 50;
    }
    if (dragTcpSpeedLimit - 1500.0 > 0.0001)
    {
        dragTcpSpeedLimit = 1500;
    }
    if (0 != iniSaveDragSpeedLimit(dragTcpSpeedLimit))
    {
        return -1;
    }
    if (0 != zucSetDragTcpSpeedLimit(dragTcpSpeedLimit))
    {
        return -1;
    }
    return 0;
}

int zucGetJointSafety(ZUC_MOTION_STAT* stat)
{
    //意义不明
    int model;
    // get robot model from serial number
    zucGetModelFromSerialNum(stat->robot_serial_num, &model);
    updateJointSafety(model, stat->robot_serial_num);
    return 0;
}

int zucSetRobotTcpVelLimit(float tcpVelLimit)
{
    try
    {
        int ret;
        auto cmd = std::make_shared<CmdSetTcpVelLimit>(0, tcpVelLimit);
        ret = mot::realtime_cmd_call(cmd);

        tcp_vel_limit_toppra = tcpVelLimit;
        if (task_init_finish_flag)
        {
            usrmotReadZucmotStatus(&zucmotStatus);
            iniChecksumPersistence(zucmotStatus.safetyCRC, CRC_FILE);
        }
        return ret;
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotTcpVelLimit function error:%s\n", e.what());
        return -1;
    }
}

int zucSetRobotStoppingDist(float stoppingDist)
{
    try
    {
        auto cmd = std::make_shared<CmdSetStoppingDist>(0, stoppingDist);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotstoppingDist function error:%s\n", e.what());
        return -1;
    }
}

int zucSetRobotStoppingTime(float stoppingTime)
{
    try
    {
        auto cmd = std::make_shared<CmdSetStoppingTime>(0, stoppingTime);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotStoppingTime function error:%s\n", e.what());
        return -1;
    }
}

int zucSetRobotElbowVelLimitReduce(double elbowVelLimitReduce)
{
    try
    {
        auto cmd = std::make_shared<CmdSetElbowVelLimitReduce>(0, elbowVelLimitReduce);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotElbowVelLimitReduce function error:%s\n", e.what());
        return -1;
    }
}

int zucSetDragTcpSpeedLimit(double dragTcpSpeedLimit)
{
    try
    {
        int ret;
        auto cmd = std::make_shared<CmdSetDragTcpSpeedLimit>(0, dragTcpSpeedLimit);
        return mot::realtime_cmd_call(cmd);
        if (task_init_finish_flag)
        {
            usrmotReadZucmotStatus(&zucmotStatus);
            iniChecksumPersistence(zucmotStatus.safetyCRC, CRC_FILE);
        }
        return ret;
    }
    catch (const std::exception& e)
    {
        printf("zucSetDragTcpSpeedLimit function error:%s\n", e.what());
        return -1;
    }
    return 0;
}

int zucSetRobotTcpVelLimitReduce(double tcpVelLimitReduce)
{
    try
    {
        auto cmd = std::make_shared<CmdSetTcpVelLimitReduce>(0, tcpVelLimitReduce);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotElbowVelLimitReduce function error:%s\n", e.what());
        return -1;
    }
}

int zucSetRobotMomentumLimitReduce(double momentumLimitReduce)
{
    try
    {
        auto cmd = std::make_shared<CmdSetMomentumLimitReduce>(0, momentumLimitReduce);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotElbowVelLimitReduce function error:%s\n", e.what());
        return -1;
    }
}

int zucSetRobotPowerLimitReduce(double robotPowerLimitReduce)
{
    try
    {
        auto cmd = std::make_shared<CmdSetRobotPowerLimitReduce>(0, robotPowerLimitReduce);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotElbowVelLimitReduce function error:%s\n", e.what());
        return -1;
    }
}

int zucSetRobotStoppingDistReduce(double stopping_dist_reduce)
{
    try
    {
        auto cmd = std::make_shared<CmdSetStoppingDistReduce>(0, stopping_dist_reduce);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotElbowVelLimitReduce function error:%s\n", e.what());
        return -1;
    }
}

int zucSetRobotStoppingTimeReduce(double stopping_time_reduce)
{
    try
    {
        auto cmd = std::make_shared<CmdSetStoppingTimeReduce>(0, stopping_time_reduce);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotElbowVelLimitReduce function error:%s\n", e.what());
        return -1;
    }
}

int zucSetRobotMomentumLimit(float momentumLimit)
{
    try
    {
        int ret;
        auto cmd = std::make_shared<CmdSetMomentumLimit>(0, momentumLimit);
        return mot::realtime_cmd_call(cmd);
        if (task_init_finish_flag)
        {
            usrmotReadZucmotStatus(&zucmotStatus);
            iniChecksumPersistence(zucmotStatus.safetyCRC, CRC_FILE);
        }
        return ret;
    }
    catch (const std::exception& e)
    {
        printf("zucSetRobotMomentumLimit function error:%s\n", e.what());
        return -1;
    }
}

int zucSetCabVoltage(unsigned int cabVoltage)
{
    try
    {
        auto cmd = std::make_shared<CmdSetCabvoltage>(cabVoltage);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("ZUCMOT_SET_CABVOLTAGE function error:%s\n", e.what());
        return -1;
    }
}

int zucSetOperationMode(unsigned int mode)
{
    try
    {
        std::string filename = ZUC_USER_CONFIG_DIR "/usersettings.ini";
        Util::IniParser ini_file(filename.c_str());
        ini_file.set("OPERATIONMODE.MODE", std::to_string(mode));
        //ini_putl("OPERATIONMODE", "MODE", mode, filename.c_str());
        syncFileToDisk(filename.c_str());
        auto cmd = std::make_shared<CmdSetOperationmode>(0, mode);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("ZUCMOT_SET_OPERATIONMODE function error:%s\n", e.what());
        return -1;
    }
}

int zucSetOperationModeOption(unsigned int enable)
{
    try
    {
        auto cmd = std::make_shared<CmdSetOperationmodeOption>(0, enable);
        return mot::realtime_cmd_call(cmd);
    }
    catch (const std::exception& e)
    {
        printf("ZUCMOT_SET_OPERATIONMODE_OPTION function error:%s\n", e.what());
        return -1;
    }
}

int zucTaskInitFinish()
{
    try
    {
        task_init_finish_flag = 1;
        // auto cmd = std::make_shared<CmdTaskInitFinish>(0);
        // return mot::realtime_cmd_call(cmd);
        return 0;
    }
    catch (const std::exception& e)
    {
        printf("ZUCMOT_SET_CABVOLTAGE function error:%s\n", e.what());
        return -1;
    }
}

int zucSetServoDynamicsPara(int robot_id, const int* servo_dynamics_para)
{
    //    int para[128] = {};
    //    // @FIXME liwang 动力学参数的适配
    //    FILE* fp = fopen(DYNAMIC_RESULT_FILE, "r");
    //    if (!fp)
    //    {
    //        rtapi_print("Identify file miss, %s!\n", DYNAMIC_RESULT_FILE);
    //        return 0;
    //    }
    //    double temp = 0;
    //    for (int i = 0; i < 84; i++) { fscanf(fp, "%lf", &temp); }  // 第一行84个数据略过
    //    for (int i = 0; i < 60; i++)
    //    {
    //        fscanf(fp, "%lf", &temp);
    //        para[i] = (int)(temp * 100000);
    //    };
    //    auto cmd = std::make_shared<CmdSetServoDynamicsPara>(robot_id, para);
    //    return mot::realtime_cmd_call(cmd);
    return 0;
}
